diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c338290c4..c01d1c536 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -556,10 +556,9 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, +ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const ISAM2UpdateParams& up) { + const ISAM2UpdateParams& updateParams) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -577,7 +576,7 @@ ISAM2Result ISAM2::update( if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = - up.force_relinearize || (params_.enableRelinearization && + updateParams.force_relinearize || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); if (verbose) { @@ -601,8 +600,8 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; - removeFactors.reserve(up.removeFactorIndices.size()); - for (const auto index : up.removeFactorIndices) { + removeFactors.reserve(updateParams.removeFactorIndices.size()); + for (const auto index : updateParams.removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -610,8 +609,8 @@ ISAM2Result ISAM2::update( // Remove removed factors from the variable index so we do not attempt to // relinearize them - variableIndex_.remove(up.removeFactorIndices.begin(), - up.removeFactorIndices.end(), + variableIndex_.remove(updateParams.removeFactorIndices.begin(), + updateParams.removeFactorIndices.end(), removeFactors); // Compute unused keys and indices @@ -666,16 +665,16 @@ ISAM2Result ISAM2::update( markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys - if (up.extraReelimKeys) { - for (Key key : *up.extraReelimKeys) { + if (updateParams.extraReelimKeys) { + for (Key key : *updateParams.extraReelimKeys) { markedKeys.insert(key); } } // Also, keys that were not observed in existing factors, but whose affected // keys have been extended now (e.g. smart factors) - if (up.newAffectedKeys) { - for (const auto &f2ks : up.newAffectedKeys.value()) { - const auto factorIdx = f2ks.first; + if (updateParams.newAffectedKeys) { + for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys(); markedKeys.insert(affectedKeys.begin(),affectedKeys.end()); } @@ -718,8 +717,8 @@ ISAM2Result ISAM2::update( for (Key key : fixedVariables_) { relinKeys.erase(key); } - if (up.noRelinKeys) { - for (Key key : *up.noRelinKeys) { + if (updateParams.noRelinKeys) { + for (Key key : *updateParams.noRelinKeys) { relinKeys.erase(key); } } @@ -798,10 +797,11 @@ ISAM2Result ISAM2::update( variableIndex_.augment(newFactors); // Augment it with existing factors which now affect to more variables: - if (up.newAffectedKeys) { - for (const auto &fk : *up.newAffectedKeys) { - const auto factorIdx = fk.first; - variableIndex_.augmentExistingFactor(factorIdx, fk.second); + if (updateParams.newAffectedKeys) { + for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + variableIndex_.augmentExistingFactor( + factorIdx, factorAddedKeys.second); } } gttoc(augment_VI); @@ -810,8 +810,9 @@ ISAM2Result ISAM2::update( // 8. Redo top of Bayes tree boost::shared_ptr replacedKeys; if (!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, - unusedIndices, up.constrainedKeys, &result); + replacedKeys = recalculate( + markedKeys, relinKeys, observedKeys, unusedIndices, + updateParams.constrainedKeys, &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 dbdffc1f4..60cdc45fe 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -158,18 +158,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { bool force_relinearize = false); /** - * Alternative signature of update() with all additional parameters in one - * structure. This form makes easier to keep future API/ABI compatibility if - * parameters change. + * Add new factors, updating the solution and relinearizing as needed. * - * @param extraParams Additional parameters to control relinearization, + * Alternative signature of update() (see its documentation above), with all + * additional parameters in one structure. This form makes easier to keep + * future API/ABI compatibility if parameters change. + * + * @param newFactors The new factors to be added to the system + * @param newTheta Initialization points for new variables to be added to the + * system. You must include here all new variables occuring in newFactors + * (which were not already in the system). There must not be any variables + * here that do not occur in newFactors, and additionally, variables that were + * already in the system must not be included here. + * @param updateParams Additional parameters to control relinearization, * constrained keys, etc. * @return An ISAM2Result struct containing information about the update * @note No default parameters to avoid ambiguous call errors. */ virtual ISAM2Result update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const ISAM2UpdateParams& up); + const ISAM2UpdateParams& updateParams); /** Marginalize out variables listed in leafKeys. These keys must be leaves * in the BayesTree. Throws MarginalizeNonleafException if non-leaves are diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 0e056b389..77f722b12 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -35,26 +35,33 @@ struct GTSAM_EXPORT ISAM2UpdateParams { /** Indices of factors to remove from system (default: empty) */ FactorIndices removeFactorIndices; - /* An optional map of keys to group labels, such that a variable can be + /** An optional map of keys to group labels, such that a variable can be * constrained to a particular grouping in the BayesTree */ boost::optional> constrainedKeys{boost::none}; - /* An optional set of nonlinear keys that iSAM2 will hold at a constant + /** An optional set of nonlinear keys that iSAM2 will hold at a constant * linearization point, regardless of the size of the linear delta */ boost::optional> noRelinKeys{boost::none}; - /* An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless + /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless * of the size of the linear delta. This allows the provided keys to be * reordered. */ boost::optional> extraReelimKeys{boost::none}; - /* Relinearize any variables whose delta magnitude is sufficiently large + /** Relinearize any variables whose delta magnitude is sufficiently large * (Params::relinearizeThreshold), regardless of the relinearization * interval (Params::relinearizeSkip). */ bool force_relinearize{false}; /** An optional set of new Keys that are now affected by factors, * indexed by factor indices (as returned by ISAM2::update()). + * Use when working with smart factors. For example: + * - Timestamp `i`: ISAM2::update() called with a new smart factor depending + * on Keys `X(0)` and `X(1)`. It returns that the factor index for the new + * smart factor (inside ISAM2) is `13`. + * - Timestamp `i+1`: The same smart factor has been augmented to now also + * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include + * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ boost::optional> newAffectedKeys{boost::none};