diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c0b2d07578..4a920a1d7c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -544,6 +544,22 @@ ISAM2Result ISAM2::update( const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { + + ISAM2UpdateParams params; + params.constrainedKeys = constrainedKeys; + params.extraReelimKeys = extraReelimKeys; + params.force_relinearize = force_relinearize; + params.noRelinKeys = noRelinKeys; + params.removeFactorIndices = removeFactorIndices; + + return update(newFactors, newTheta, params); +} + +/* ************************************************************************* */ +ISAM2Result ISAM2::update( + const NonlinearFactorGraph& newFactors, + const Values& newTheta, + const ISAM2UpdateParams& up) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -561,7 +577,7 @@ ISAM2Result ISAM2::update( if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = - force_relinearize || (params_.enableRelinearization && + up.force_relinearize || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); if (verbose) { @@ -585,8 +601,8 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; - removeFactors.reserve(removeFactorIndices.size()); - for (const auto index : removeFactorIndices) { + removeFactors.reserve(up.removeFactorIndices.size()); + for (const auto index : up.removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -594,7 +610,8 @@ ISAM2Result ISAM2::update( // Remove removed factors from the variable index so we do not attempt to // relinearize them - variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), + variableIndex_.remove(up.removeFactorIndices.begin(), + up.removeFactorIndices.end(), removeFactors); // Compute unused keys and indices @@ -649,8 +666,8 @@ ISAM2Result ISAM2::update( markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys - if (extraReelimKeys) { - for (Key key : *extraReelimKeys) { + if (up.extraReelimKeys) { + for (Key key : *up.extraReelimKeys) { markedKeys.insert(key); } } @@ -695,8 +712,8 @@ ISAM2Result ISAM2::update( for (Key key : fixedVariables_) { relinKeys.erase(key); } - if (noRelinKeys) { - for (Key key : *noRelinKeys) { + if (up.noRelinKeys) { + for (Key key : *up.noRelinKeys) { relinKeys.erase(key); } } @@ -780,7 +797,7 @@ ISAM2Result ISAM2::update( boost::shared_ptr replacedKeys; if (!markedKeys.empty() || !observedKeys.empty()) replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, - unusedIndices, constrainedKeys, &result); + unusedIndices, up.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 4b8b1398ba..dbdffc1f42 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -156,6 +157,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const boost::optional >& extraReelimKeys = boost::none, 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. + * + * @param extraParams 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); + /** Marginalize out variables listed in leafKeys. These keys must be leaves * in the BayesTree. Throws MarginalizeNonleafException if non-leaves are * requested to be marginalized. Marginalization leaves a linear diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h new file mode 100644 index 0000000000..31ec8d560e --- /dev/null +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2UpdateParams.h + * @brief Class that stores extra params for ISAM2::update() + * @author Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco + */ + +// \callgraph + +#pragma once + +#include +#include +#include // GTSAM_EXPORT +#include // Key, KeySet +#include //FactorIndices + +namespace gtsam { + +/** + * @addtogroup ISAM2 + * This struct is used by ISAM2::update() to pass additional parameters to + * give the user a fine-grained control on how factors and relinearized, etc. + */ +struct GTSAM_EXPORT ISAM2UpdateParams { + ISAM2UpdateParams() = default; + + /** 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 + * 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 + * 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 + * 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 + * (Params::relinearizeThreshold), regardless of the relinearization + * interval (Params::relinearizeSkip). */ + bool force_relinearize{false}; +}; + +} // namespace gtsam