From 14c3467520851a0520b4a4ce8f8eadb806c4e82d Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Thu, 31 Oct 2024 19:05:54 +0800
Subject: [PATCH] Remove old marginalization code in
 IncrementalFixedLagSmoother.cpp

---
 .../nonlinear/IncrementalFixedLagSmoother.cpp | 37 -------------------
 1 file changed, 37 deletions(-)

diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index afe4fb3de1..238ff6b3d3 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -23,34 +23,8 @@
 #include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
 #include <gtsam/base/debug.h>
 
-// #define GTSAM_OLD_MARGINALIZATION
-
 namespace gtsam {
 
-/* ************************************************************************* */
-#ifdef GTSAM_OLD_MARGINALIZATION
-void recursiveMarkAffectedKeys(const Key& key,
-    const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
-
-  // Check if the separator keys of the current clique contain the specified key
-  if (std::find(clique->conditional()->beginParents(),
-      clique->conditional()->endParents(), key)
-      != clique->conditional()->endParents()) {
-
-    // Mark the frontal keys of the current clique
-    for(Key i: clique->conditional()->frontals()) {
-      additionalKeys.insert(i);
-    }
-
-    // Recursively mark all of the children
-    for(const ISAM2Clique::shared_ptr& child: clique->children) {
-      recursiveMarkAffectedKeys(key, child, additionalKeys);
-    }
-  }
-  // If the key was not found in the separator/parents, then none of its children can have it either
-}
-#endif
-
 /* ************************************************************************* */
 void IncrementalFixedLagSmoother::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
@@ -119,20 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     std::cout << std::endl;
   }
 
-  // Mark additional keys between the marginalized keys and the leaves
-#ifdef GTSAM_OLD_MARGINALIZATION
-  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);
-    }
-  }
-#else
   std::set<Key> additionalKeys =
       BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
           isam_, marginalizableKeys);
-#endif
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 
   // Update iSAM2