diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 423a576c1..3dc0b430d 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -552,7 +552,8 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, bool force_relinearize) { + const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, + const boost::optional >& extraReelimKeys, bool force_relinearize) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -648,6 +649,12 @@ ISAM2Result ISAM2::update( FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } + // Also mark any provided extra re-eliminate keys + if(extraReelimKeys) { + BOOST_FOREACH(Key key, *extraReelimKeys) { + markedKeys.insert(ordering_.at(key)); + } + } // Observed keys for detailed results if(params_.enableDetailedResults) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 71a07e228..3b4bbaa48 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -544,12 +544,17 @@ public: * (Params::relinearizeSkip). * @param constrainedKeys is an optional map of keys to group labels, such that a variable can * be constrained to a particular grouping in the BayesTree + * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization + * point, regardless of the size of the linear delta + * @param extraReelimKeys is 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. * @return An ISAM2Result struct containing information about the update */ GTSAM_EXPORT ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, + const boost::optional >& extraReelimKeys = boost::none, bool force_relinearize = false); /** Marginalize out variables listed in leafKeys. These keys must be leaves