IncrementalFixedLagSmoother still throws due to unused keys

[Ellon]

Description

NOTE: This bug is only exposed after fixing issue
#2478

Whenever a call to IncrementalFixedLagSmoother::update triggers
marginalization, if some values becomes unused in ISAM2 during the update (due
to factors being removed in the update and thus "disconnecting" some values
from the graph), an 'std::out_of_range' exception will be thrown from an invalid
call to map::at.

Expected behavior

Do not throw the exception in this case.

Steps to reproduce

1. Setup an IncrementalFixedLagSmoother

2. Update it adding some values and factors so it is about marginalize some
   values

3. Make another update that at the same time:

   • Triggers a marginalization; and

   • Make previously added values unused, by removing factors that connected
     them to the rest of the graph.

Reason

When marginalizing, the IncrementalFixedLagSmoother will set up the
constrainedKeys map by calling createOrderingConstraints(marginalizableKeys, constrainedKeys);. This method uses the key-timestamp maps set groups to all
known keys, putting the keys to be marginalized on the left (group 0) and the
rest on the right (group 1). At this stage, IncrementalFixedLagSmoother does
not know that some of these keys will get unused during the ISAM2 update. Later,
deep inside ISAM2, when computing an ordering while doing a batch recalculate
with constrained keys, ISAM2 will try to access a key in a std::map with a key
coming from the informed constrainedKeys, that won't exist anymore because it
is unused. Precisely, the at is on gtsam/inference/Ordering.cpp:204.

Possible Solutions

The easiest solution is to IMO is to add a check inside ISAM2 that if
constrainedKeys is set and we have unused keys, remove any unused keys from
the group before continuing. Even if this looks like a fix in ISAM2 from a
problem coming from IncrementalFixedLagSmoother, similar logic exists already
in the ISAM2 code (look for result->unusedKeys inside
ISAM2::recalculateIncremental), so it seems reasonable to me. This is the
solution proposed on #2474.

On the other hand, one could consider that constrainedKeys passed to a ISAM2
update call must not contain keys that will become unused. In this case, the
solution would be to add some way for IncrementalFixedLagSmoother to know in
advance that a current valid key will get unused during the update, and prevent
it to be added to constrainedKeys. I consider this to be more complicated to
implement.

Minimal (Non-)Working Example

NOTE: This example only exposes the bug after fixing issue
#2478

#include <gtsam/base/debug.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/slam/BetweenFactor.h>

int main() {
  using namespace gtsam;
  using symbol_shorthand::X;

  SETDEBUG("IncrementalFixedLagSmoother update", true);

  SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));

  typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
  IncrementalFixedLagSmoother smoother(5.0, ISAM2Params());

  size_t i = 0;

  // Add a prior at time 0
  {
    Key key0 = X(0);

    NonlinearFactorGraph newFactors;
    Values newValues;
    Timestamps newTimestamps;

    newFactors.addPrior(key0, Point2(0.0, 0.0), noise);
    newValues.insert(key0, Point2(0.01, 0.01));
    newTimestamps[key0] = 0.0;

    smoother.update(newFactors, newValues, newTimestamps);

    ++i;
  }

  FactorIndices factorsToRemove;
  size_t ref_i = 0;

  // Helper lambda to setup an usage of the smoother where we add new values and
  // update at a certain frequency, but do not keep all added values in the
  // smoother in a long term. This is achieved by storing the indexes to the new
  // factors being added, and using them to remove the factors later. The
  // removal of factors may cause keys to be unused and removed from the
  // smoother.
  const auto add_x_keep_every_y = [&](size_t n_steps, size_t keep_interval) {
    for (auto j = 0ul; j < n_steps; j++) {
      Key key1 = X(ref_i);
      Key key2 = X(i);

      NonlinearFactorGraph newFactors;
      Values newValues;
      Timestamps newTimestamps;

      newFactors.push_back(
          BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), noise));
      newFactors.addPrior(key2, Point2(double(i), 0.0), noise);
      newValues.insert(key2, Point2(double(i) + 0.1, -0.1));
      newTimestamps[key2] = double(i);

      smoother.update(newFactors, newValues, newTimestamps, factorsToRemove);

      // Store indexes of new factors so we're able to remove them later, if
      // needed.
      factorsToRemove = smoother.getISAM2Result().newFactorsIndices;

      // Decide if we want to keep the new value in the smoother
      if ((j + 1) % keep_interval == 0) {
        // We want to keep it. Clear factorsToRemove so newValue stays
        // connected to the graph.
        factorsToRemove.clear();
        // Store new reference
        ref_i = i;
      }

      ++i;
    }
  };

  // Add odometry from time 0 to time 9, with a prior factor on each new value.
  // Keep every third new value.
  //
  // When adding factors to x9, x3 will be marginalized at the same time that x8
  // will be become unused. This will trigger the exception.
  add_x_keep_every_y(9, 3);

  return 0;
}

Output

IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
{Empty Tree}
END
Current Timestamp: 0
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 )
END
Final Bayes Tree:
P( x0 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 )
END
Current Timestamp: 1
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x1 )
END
Final Bayes Tree:
P( x0 x1 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x1 )
END
Current Timestamp: 2
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x2 )
END
Final Bayes Tree:
P( x0 x2 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x2 )
END
Current Timestamp: 3
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x3 )
END
Final Bayes Tree:
P( x0 x3 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x3 )
END
Current Timestamp: 4
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x3 x4 )
 P( x0 | x3 )
END
Final Bayes Tree:
P( x3 x4 )
 P( x0 | x3 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x3 x4 )
 P( x0 | x3 )
END
Current Timestamp: 5
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x3 x5 )
 P( x0 | x3 )
END
Final Bayes Tree:
P( x3 x5 )
 P( x0 | x3 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x3 x5 )
 P( x0 | x3 )
END
Current Timestamp: 6
Marginalizable Keys: x0 
Constrained Keys: x0(0)  x3(1)  x5(1)  x6(1)  
Bayes Tree After Update, Before Marginalization:
P( x3 x6 )
 P( x0 | x3 )
END
Final Bayes Tree:
P( x3 x6 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x3 x6 )
END
Current Timestamp: 7
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x6 x7 )
 P( x3 | x6 )
END
Final Bayes Tree:
P( x6 x7 )
 P( x3 | x6 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x6 x7 )
 P( x3 | x6 )
END
Current Timestamp: 8
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x6 x8 )
 P( x3 | x6 )
END
Final Bayes Tree:
P( x6 x8 )
 P( x3 | x6 )
END
IncrementalFixedLagSmoother::update() Finish
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x6 x8 )
 P( x3 | x6 )
END
Current Timestamp: 9
Marginalizable Keys: x3 
Constrained Keys: x3(0)  x6(1)  x8(1)  x9(1)  
terminate called after throwing an instance of 'std::out_of_range'
  what():  map::at
Aborted (core dumped)

Environment

Fedora 42, C++. Bug confirmed to be present at least on on 4.2a9, 4.3a0 and
current develop (commit 75b421980)

[Ellon]

Fixed on #2474