Some last cosmetic changes to make update fit on single screen
							parent
							
								
									ebae189c7b
								
							
						
					
					
						commit
						d18795e844
					
				|  | @ -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); | ||||
|  |  | |||
|  | @ -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; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -157,6 +157,14 @@ struct GTSAM_EXPORT ISAM2Result { | |||
|    * Detail for information about the results data stored here. */ | ||||
|   boost::optional<DetailedResults> 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 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue