Marginalizing in IncrementalFixedLagSmoother sometime leaves the marginalized key in the BayesTree

[etamarlu]

I use IncrementalFixedLagSmoother for a VO problem with a "sliding window", when a new frame is added, either the 1st frame in the sliding window is marginalized, or the previous one.
I do it by:

1. changing the Lag
2. changing the timestamp for the previous frame if it is to be marginalized.

It works fine most of the time.

At some point when I do marginalization for the previous frame - Its place in the Bayes Tree (as seen by the Bayes Tree symbolic print in the debug output) does not change.

If the algorithm runs another iteration an error occurs.
The error happens in ISAM2-impl.h, 'GetAffectedFactors' function when calling "variableIndex[key]". when back tracing where key gets the missing key value - key gets this from affectedBayesNet that is created in 'ISAM2::recalculate' in

    this->removeTop(
        KeyVector(result->markedKeys.begin(), result->markedKeys.end()),
        &affectedBayesNet, &orphans);

I understand this bug report may not be very helpful. If it helps I can send the files to reproduce it.