Fix variable marginalization for IncrementalFixedLagSmoother

[NewThinker-Jeffrey]

This pull request fixes a bug in the marginalization of IncrementalFixedLagSmoother.

If the variable to be marginalized, say xi,

1. is not in a leaf node
2. and no children node rely on xi,
3. and there's another non-marginalizable frontal variable, say xj, before xi in the same node

then IncrementalFixedLagSmoother can not marginalize the variable xi from the underlying Bayes tree but it removes the value for xi , which causes exceptions like:

terminate called after throwing an instance of 'std::out_of_range'
  what():  Requested variable 'x28' is not in this VectorValues.
*** Aborted at 1729685632 (unix time) try "date -d @1729685632" if you are using GNU date ***
    @     0xffffa42ae7c0 ([vdso]+0x7bf)
    @     0xffff93eb5d78 gsignal
    @     0xffff93ea2aac abort
    @     0xffff940bc8bc __gnu_cxx::__verbose_terminate_handler()
    @     0xffff940ba20c (unknown)
    @     0xffff940ba270 std::terminate()
    @     0xffff940ba564 __cxa_throw
    @     0xffffa3677318 gtsam::VectorValues::at()
    @     0xffff949a3f2c gtsam::VectorValues::vector<>()
    @     0xffff949a0028 gtsam::ISAM2Clique::optimizeWildfireNode()
    @     0xffff949a043c gtsam::optimizeWildfireNonRecursive()
    @     0xffff9497b424 gtsam::DeltaImpl::UpdateGaussNewtonDelta()
    @     0xffff94980f6c gtsam::ISAM2::updateDelta()
    @     0xffff94984ffc gtsam::ISAM2::getDelta()
    @     0xffffa3676fb0 gtsam::ISAM2::calculateEstimate<>()

terminate called after throwing an instance of 'std::invalid_argument'
  what():  Requested non-existent variable from VariableIndex
*** Aborted at 1729686609 (unix time) try "date -d @1729686609" if you are using GNU date ***
    @     0xffffaa3bc7c0 ([vdso]+0x7bf)
    @     0xffff99fc3d78 gsignal
    @     0xffff99fb0aac abort
    @     0xffff9a1ca8bc __gnu_cxx::__verbose_terminate_handler()
    @     0xffff9a1c820c (unknown)
    @     0xffff9a1c8270 std::terminate()
    @     0xffff9a1c8564 __cxa_throw
    @     0xffff9aa9d0ac gtsam::UpdateImpl::GetAffectedFactors()
    @     0xffff9aa8b3cc gtsam::ISAM2::relinearizeAffectedFactors()
    @     0xffff9aa8d234 gtsam::ISAM2::recalculateIncremental()
    @     0xffff9aa8be60 gtsam::ISAM2::recalculate()
    @     0xffff9aa8ede0 gtsam::ISAM2::update()
    @     0xffff9aa8ea10 gtsam::ISAM2::update()
    @     0xffff9ad6e84c gtsam::IncrementalFixedLagSmoother::update()

The bug stems from this piece of code,

  // Mark additional keys between the marginalized keys and the leaves
  std::set<Key> additionalKeys;
  for(Key key: marginalizableKeys) {
    ISAM2Clique::shared_ptr clique = isam_[key];
    for(const ISAM2Clique::shared_ptr& child: clique->children) {
      recursiveMarkAffectedKeys(key, child, additionalKeys);
    }
  }
  KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());

When a variable in marginalizableKeys is in the situations listed above, it needs re-elimination so that it can be eliminated before other variables. But with the code above it won't be added to the set additionalKeys, and consequently that variable won't be re-eliminated in the subsequent isam update.

For a specific example of the bug, see the Bayes tree below and consider the variable x42:

P( x48 x49 )
 P( x43 x42 | x48 )   # <-- 1. x42 is not in a leaf node;  2. no children node relies on x42;  3. there is another frontal x43 before x42;
  P( x46 | x43 x48 )
   P( x45 | x43 x46 )
    P( x44 | x43 x45 )
   P( x47 | x46 x48 )
 P( x50 | x49 )
  P( x51 | x50 )
   P( x52 | x51 )
    P( x53 | x52 )
     P( x54 | x53 )

In this case, if we marginalize x42 (put it in marginalizableKeys), the code above won't add any variable to additionalKeys , and x42 will not be re-eliminated to become a "leaf key". ISAM2::marginalizeLeaves() can't handle this corner case correctly (it doesn't effectively marginalize the variable from the tree, but remove the value and factor indices of the variable).

The example above can be reproduced with the modified unit test for IncrementalFixedLagSmoother in this PR. See testIncrementalFixedLagSmoother.cpp

[NewThinker-Jeffrey]

About the fix:

In this PR, I created a new header file BayesTreeMarginalizationHelper.h and added a helper function gatherAdditionalKeysToReEliminate() to decide the additional keys that need re-elimination. Please see the code and doc string of that function for the implementation details:

Note the old marginalization code are still kept, but disabled. To compare the effects of old and the new code, you can enable / disable the macro GTSAM_OLD_MARGINALIZATION to switch between the two versions.

When the macro GTSAM_OLD_MARGINALIZATION is defined, the old marginalization code will be activated and the unit test testIncrementalFixedLagSmoother will fail in the marginalization test case, as described above; while with the new marginalization in this PR, the tests are stable.

[dellaert]

Wow, this must be one of the most thorough PRs I have seen :-) Many thanks!
Two requests:

• I trust you: just remove the "ifdefery" :-)
• the main function is too long. I asked Claude 3.5 for a re-factor. Could you try that on your end and use that (or something similar) if you're convinced it's correct?

[dellaert]

This function (and its docs) are too long to understand well. In general I'd like to adhere to a rule "no function should be longer than 4 lines and never more than a page" :-) We violate that a lot. I asked Claude 3.5 to refactor (and also return the keys, rather than using a non-const reference) and it came up with this:

class BayesTreeMarginalizationHelper {
private:
  /**
   * Check if a clique contains variables that need reelimination due to
   * marginalization ordering conflicts.
   * 
   * @param[in] clique The clique to check
   * @param[in] marginalizableKeys Set of keys to be marginalized
   * @return true if any variables in the clique need re-elimination
   */
  static bool needsReelimination(
      const sharedClique& clique,
      const std::set<Key>& marginalizableKeys) {
    bool hasNonMarginalizableAhead = false;
    
    // Check each frontal variable in order
    for (Key key : clique->conditional()->frontals()) {
      if (marginalizableKeys.count(key)) {
        // If we've seen non-marginalizable variables before this one,
        // we need to reeliminate
        if (hasNonMarginalizableAhead) {
          return true;
        }
        
        // Check if any child depends on this marginalizable key
        if (hasChildDependency(clique, key)) {
          return true;
        }
      } else {
        hasNonMarginalizableAhead = true;
      }
    }
    return false;
  }

  /**
   * Check if any child clique depends on the given key.
   * 
   * @param[in] clique Parent clique to check
   * @param[in] key Key to check for dependencies
   * @return true if any child depends on the key
   */
  static bool hasChildDependency(
      const sharedClique& clique,
      Key key) {
    for (const sharedClique& child : clique->children) {
      auto parents = child->conditional();
      if (std::find(parents->beginParents(),
          parents->endParents(), key)
          != parents->endParents()) {
        return true;
      }
    }
    return false;
  }

  /**
   * Gather all dependent cliques that need re-elimination based on a root clique.
   * 
   * @param[in] rootClique The starting clique
   * @param[in] marginalizableKeys Set of keys to be marginalized
   * @param[out] dependentCliques Pointer to set storing dependent cliques
   * @param[out] additionalKeys Pointer to set storing keys that need reelimination
   */
  static void gatherDependentCliques(
      const sharedClique& rootClique,
      const std::set<Key>& marginalizableKeys,
      std::set<sharedClique>* dependentCliques,
      std::set<Key>* additionalKeys) {
    // Add frontal variables from current clique
    for (Key key : rootClique->conditional()->frontals()) {
      additionalKeys->insert(key);
      // Find children that depend on this key
      for (const sharedClique& child : rootClique->children) {
        if (!dependentCliques->count(child) &&
            hasChildDependency(rootClique, key)) {
          dependentCliques->insert(child);
        }
      }
    }
  }

  /**
   * Process all dependent cliques and gather their keys.
   * 
   * @param[in,out] dependentCliques Pointer to set of cliques to process
   * @return Set of keys from all dependent cliques
   */
  static std::set<Key> processDependentCliques(
      std::set<sharedClique>* dependentCliques) {
    std::set<Key> additionalKeys;
    
    while (!dependentCliques->empty()) {
      auto begin = dependentCliques->begin();
      sharedClique clique = *begin;
      dependentCliques->erase(begin);

      // Add all frontal variables from this clique
      for (Key key : clique->conditional()->frontals()) {
        additionalKeys.insert(key);
      }

      // Add all children for processing
      for (const sharedClique& child : clique->children) {
        dependentCliques->insert(child);
      }
    }
    
    return additionalKeys;
  }

public:
  /**
   * Get additional keys that need re-elimination when marginalizing variables.
   * 
   * This function identifies variables that need to be re-eliminated to ensure
   * proper marginalization order in two cases:
   * 
   * 1. When a child node depends on a variable being marginalized
   * 2. When non-marginalizable variables appear before marginalized ones
   * 
   * @param[in] bayesTree The Bayes tree
   * @param[in] marginalizableKeys Keys to be marginalized
   * @return Set of additional keys that need to be re-eliminated
   */
  static std::set<Key> gatherAdditionalKeysToReEliminate(
      const BayesTree& bayesTree,
      const KeyVector& marginalizableKeys) {
    const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
    
    std::set<Key> marginalizableKeySet(marginalizableKeys.begin(), 
                                     marginalizableKeys.end());
    std::set<sharedClique> checkedCliques;
    std::set<sharedClique> dependentCliques;
    std::set<Key> additionalKeys;

    // Check each marginalizable key's clique
    for (const Key& key : marginalizableKeySet) {
      sharedClique clique = bayesTree[key];
      if (checkedCliques.count(clique)) continue;
      
      checkedCliques.insert(clique);
      
      if (needsReelimination(clique, marginalizableKeySet)) {
        gatherDependentCliques(clique, marginalizableKeySet, 
                             &dependentCliques, &additionalKeys);
      }
    }

    // Process remaining dependent cliques
    auto dependentKeys = processDependentCliques(&dependentCliques);
    additionalKeys.insert(dependentKeys.begin(), dependentKeys.end());

    if (debug) {
      std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
      for (const Key& key : additionalKeys) {
        std::cout << DefaultKeyFormatter(key) << " ";
      }
      std::cout << std::endl;
    }

    return additionalKeys;
  }
};