diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index b6e5663bf..7e17ec785 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -140,14 +140,15 @@ struct GTSAM_EXPORT UpdateImpl { NonlinearFactorGraph* nonlinearFactors, GaussianFactorGraph* linearFactors, VariableIndex* variableIndex, - ISAM2Result* result) const { + FactorIndices* newFactorsIndices, + KeySet* keysWithRemovedFactors) const { gttic(pushBackFactors); // Perform the first part of the bookkeeping updates for adding new factors. // Adds them to the complete list of nonlinear factors, and populates the // list of new factor indices, both optionally finding and reusing empty // factor slots. - result->newFactorsIndices = nonlinearFactors->add_factors( + *newFactorsIndices = nonlinearFactors->add_factors( newFactors, params_.findUnusedFactorSlots); // Remove the removed factors @@ -164,37 +165,41 @@ struct GTSAM_EXPORT UpdateImpl { variableIndex->remove(updateParams_.removeFactorIndices.begin(), updateParams_.removeFactorIndices.end(), removedFactors); - result->keysWithRemovedFactors = removedFactors.keys(); + *keysWithRemovedFactors = removedFactors.keys(); + } - // Compute unused keys and indices - // Get keys from removed factors and new factors, and compute unused keys, - // i.e., keys that are empty now and do not appear in the new factors. + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + void computeUnusedKeys(const NonlinearFactorGraph& newFactors, + const VariableIndex& variableIndex, + const KeySet& keysWithRemovedFactors, + KeySet* unusedKeys) const { + gttic(computeUnusedKeys); KeySet removedAndEmpty; - for (Key key : result->keysWithRemovedFactors) { - if (variableIndex->empty(key)) + for (Key key : keysWithRemovedFactors) { + if (variableIndex.empty(key)) removedAndEmpty.insert(removedAndEmpty.end(), key); } KeySet newFactorSymbKeys = newFactors.keys(); - std::set_difference( - removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), - std::inserter(result->unusedKeys, result->unusedKeys.end())); + std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(*unusedKeys, unusedKeys->end())); } // Mark linear update void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, const NonlinearFactorGraph& nonlinearFactors, - ISAM2Result* result) const { + KeySet* markedKeys, ISAM2Result* result) const { gttic(gatherInvolvedKeys); - result->markedKeys = newFactors.keys(); // Get keys from new factors + *markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors - result->markedKeys.insert(result->keysWithRemovedFactors.begin(), - result->keysWithRemovedFactors.end()); + markedKeys->insert(result->keysWithRemovedFactors.begin(), + result->keysWithRemovedFactors.end()); // Also mark any provided extra re-eliminate keys if (updateParams_.extraReelimKeys) { for (Key key : *updateParams_.extraReelimKeys) { - result->markedKeys.insert(key); + markedKeys->insert(key); } } @@ -204,18 +209,18 @@ struct GTSAM_EXPORT UpdateImpl { for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { const auto factorIdx = factorAddedKeys.first; const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); - result->markedKeys.insert(affectedKeys.begin(), affectedKeys.end()); + markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); } } // Observed keys for detailed results if (params_.enableDetailedResults) { - for (Key key : result->markedKeys) { + for (Key key : *markedKeys) { result->detail->variableStatus[key].isObserved = true; } } - for (Key index : result->markedKeys) { + for (Key index : *markedKeys) { // Only add if not unused if (result->unusedKeys.find(index) == result->unusedKeys.end()) // Make a copy of these, as we'll soon add to them @@ -347,7 +352,7 @@ struct GTSAM_EXPORT UpdateImpl { // Mark keys in \Delta above threshold \beta: KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, const VectorValues& delta, - const KeySet& fixedVariables, + const KeySet& fixedVariables, KeySet* markedKeys, ISAM2Result* result) const { gttic(gatherRelinearizeKeys); // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. @@ -378,18 +383,18 @@ struct GTSAM_EXPORT UpdateImpl { } // Add the variables being relinearized to the marked keys - result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); + markedKeys->insert(relinKeys.begin(), relinKeys.end()); return relinKeys; } // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - ISAM2Result* result) const { + KeySet* markedKeys, ISAM2Result* result) const { gttic(fluidFindAll); for (const auto& root : roots) // add other cliques that have the marked ones in the separator - root->findAll(relinKeys, &result->markedKeys); + root->findAll(relinKeys, markedKeys); // Relinearization-involved keys for detailed results if (params_.enableDetailedResults) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c1ff1ad02..ce73706a0 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -350,11 +350,9 @@ void ISAM2::addVariables( const Values& newTheta, ISAM2Result::DetailedResults::StatusMap* variableStatus) { gttic(addNewVariables); - // \Theta:=\Theta\cup\Theta_{new}. - const bool debug = ISDEBUG("ISAM2 AddVariables"); theta_.insert(newTheta); - if (debug) newTheta.print("The new variables are: "); + if (ISDEBUG("ISAM2 AddVariables")) newTheta.print("The new variables are: "); // Add zeros into the VectorValues delta_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors()); @@ -425,9 +423,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 1. Add any new factors \Factors:=\Factors\cup\Factors'. update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, - &variableIndex_, &result); + &variableIndex_, &result.newFactorsIndices, + &result.keysWithRemovedFactors); + update.computeUnusedKeys(newFactors, variableIndex_, + result.keysWithRemovedFactors, &result.unusedKeys); // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); gttic(evaluate_error_before); @@ -436,19 +438,20 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, gttoc(evaluate_error_before); // 3. Mark linear update - update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &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) { // 4. Mark keys in \Delta above threshold \beta: - relinKeys = - update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result); + relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, + &result.markedKeys, &result); if (!relinKeys.empty()) { // 5. Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result); + update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); @@ -479,8 +482,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet, relinKeys, &orphans, &result); - // Update replaced keys mask (accumulates until back-substitution takes - // place) + // Update replaced keys mask (accumulates until back-substitution happens) deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8d8299d81..e2b9a18e8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -281,7 +281,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: -// Do a batch step - reorder and relinearize all variables + // Do a batch step - reorder and relinearize all variables void recalculateBatch(const ISAM2UpdateParams& updateParams, KeySet* affectedKeysSet, ISAM2Result* result);