diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 560e48fa05..ec543e45fc 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -119,9 +119,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; // Mark all frontal keys of the current clique. - for(Key i: clique->conditional()->frontals()) { - additionalKeys.insert(i); - } + additionalKeys.insert(clique->conditional()->frontals().begin(), + clique->conditional()->frontals().end()); // Recursively mark all of the children key that contain the marginal key. for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys);