diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 6acdd28d5..eb2285b28 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -134,6 +134,14 @@ struct GTSAM_EXPORT UpdateImpl { } } + // Check relinearization if we're at the nth step, or we are using a looser + // loop relinerization threshold. + bool relinarizationNeeded(size_t update_count) const { + return updateParams_.force_relinearize || + (params_.enableRelinearization && + update_count % params_.relinearizeSkip == 0); + } + // Add any new factors \Factors:=\Factors\cup\Factors'. void pushBackFactors(const NonlinearFactorGraph& newFactors, NonlinearFactorGraph* nonlinearFactors, @@ -406,10 +414,10 @@ struct GTSAM_EXPORT UpdateImpl { // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - KeySet* markedKeys, - ISAM2Result::DetailedResults* detail) const { - gttic(fluidFindAll); + void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys, + KeySet* markedKeys, + ISAM2Result::DetailedResults* detail) const { + gttic(findFluid); for (const auto& root : roots) // add other cliques that have the marked ones in the separator root->findAll(relinKeys, markedKeys); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index fa765a109..f56b23777 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -415,23 +415,14 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); - this->update_count_++; + this->update_count_ += 1; UpdateImpl::LogStartingUpdate(newFactors, *this); - - const bool relinearizeThisStep = - updateParams.force_relinearize || - (params_.enableRelinearization && - update_count_ % params_.relinearizeSkip == 0); + ISAM2Result result(params_.enableDetailedResults); + UpdateImpl update(params_, updateParams); // Update delta if we need it to check relinearization later - if (relinearizeThisStep) { + if (update.relinarizationNeeded(update_count_)) updateDelta(updateParams.forceFullSolve); - } - - ISAM2Result result; - if (params_.enableDetailedResults) - result.detail = ISAM2Result::DetailedResults(); - UpdateImpl update(params_, updateParams); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, @@ -442,8 +433,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.detail.get_ptr()); - + addVariables(newTheta, result.details()); if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); @@ -452,28 +442,22 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, result.keysWithRemovedFactors, &result.markedKeys); update.updateKeys(result.markedKeys, &result); - // Check relinearization if we're at the nth step, or we are using a looser - // loop relinerization threshold. KeySet relinKeys; - if (relinearizeThisStep) { + result.variablesRelinearized = 0; + if (update.relinarizationNeeded(update_count_)) { // 4. Mark keys in \Delta above threshold \beta: relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result.markedKeys); - update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr()); + update.recordRelinearizeDetail(relinKeys, result.details()); if (!relinKeys.empty()) { - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result.markedKeys, - result.detail.get_ptr()); + // 5. Mark cliques that involve marked variables \Theta_{J} and ancestors. + update.findFluid(roots_, relinKeys, &result.markedKeys, result.details()); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); } result.variablesRelinearized = result.markedKeys.size(); - } else { - result.variablesRelinearized = 0; } - // TODO(frank): should be result.variablesRelinearized = relinKeys.size(); ? // 7. Linearize new factors update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), @@ -481,18 +465,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.augmentVariableIndex(newFactors, result.newFactorsIndices, &variableIndex_); - // 8. Redo top of Bayes tree + // 8. Redo top of Bayes tree and update data structures recalculate(updateParams, relinKeys, &result); - - // Update data structures to remove unused keys - if (!result.unusedKeys.empty()) { - removeVariables(result.unusedKeys); - } + if (!result.unusedKeys.empty()) removeVariables(result.unusedKeys); result.cliques = this->nodes().size(); if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter); - return result; } diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 1287b0e1a..3953c760b 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -157,6 +157,14 @@ struct GTSAM_EXPORT ISAM2Result { * Detail for information about the results data stored here. */ boost::optional detail; + explicit ISAM2Result(bool enableDetailedResults = false) { + if (enableDetailedResults) detail.reset(DetailedResults()); + } + + /// Return pointer to detail, 0 if no detail requested + DetailedResults* details() { return detail.get_ptr(); } + + /// Print results void print(const std::string str = "") const { using std::cout; cout << str << " Reelimintated: " << variablesReeliminated