diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 9b8cc38f0..ed4b66616 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -371,9 +371,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, marginalizeKeys) { const FastList& slots = variableIndex[key]; - BOOST_FOREACH(size_t slot, slots) { - removedFactorSlots.insert(slot); - } + removedFactorSlots.insert(slots.begin(), slots.end()); } if(debug) {