From 71a7ddb3541506665d2f989e8b785ccac7974d9c Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Thu, 1 Sep 2011 01:00:21 +0000
Subject: [PATCH] removed code for older WAFR algorithm

---
 gtsam/nonlinear/ISAM2-inl.h | 23 -----------------------
 1 file changed, 23 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 9f879bdbb..f3199921c 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -26,9 +26,6 @@ using namespace boost::assign;
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/ISAM2-impl-inl.h>
 
-// for WAFR paper, separate update and relinearization steps if defined
-//#define SEPARATE_STEPS
-
 
 namespace gtsam {
 
@@ -311,14 +308,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(const
     toc(1,"relinearizeAffected");
 
 #ifndef NDEBUG
-#ifndef SEPARATE_STEPS
     // The relinearized variables should not appear anywhere in the orphans
     BOOST_FOREACH(boost::shared_ptr<const typename BayesTree<Conditional>::Clique> clique, orphans) {
       BOOST_FOREACH(const Index key, (*clique)->frontals()) {
         assert(lastRelinVariables_[key] == false);
       }
     }
-#endif
 #endif
 
     //  if(debug) factors.print("Affected factors: ");
@@ -352,11 +347,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(const
     // Copy so that we can later permute factors
     BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) {
 #ifndef NDEBUG
-#ifndef SEPARATE_STEPS
       BOOST_FOREACH(const Index key, *cached) {
         assert(lastRelinVariables_[key] == false);
       }
-#endif
 #endif
       factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached)));
     }
@@ -725,17 +718,6 @@ void ISAM2<Conditional, Values>::update(
   }
   toc(3,"gather involved keys");
 
-#ifdef SEPARATE_STEPS // original algorithm from paper: separate relin and optimize
-
-  // todo: kaess - don't need linear factors here, just to update variableIndex
-  boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_, ordering_);
-  variableIndex_.augment(*linearFactors);
-
-  boost::shared_ptr<set<Index> > replacedKeys_todo = recalculate(markedKeys, newKeys, linearFactors);
-  markedKeys.clear();
-  vector<bool> none(variableIndex_.size(), false);
-  optimize2(this->root(), wildfire_threshold, none, delta_);
-#endif
 
   vector<bool> markedRelinMask(ordering_.nVars(), false);
   bool relinAny = false;
@@ -792,7 +774,6 @@ void ISAM2<Conditional, Values>::update(
   lastRelinVariables_ = markedRelinMask;
 #endif
 
-#ifndef SEPARATE_STEPS
   tic(7,"linearize new");
   tic(1,"linearize");
   // 7. Linearize new factors
@@ -811,10 +792,6 @@ void ISAM2<Conditional, Values>::update(
   if(markedKeys.size() > 0 || newKeys.size() > 0)
     replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors);
   toc(8,"recalculate");
-#else
-  vector<Index> empty;
-  boost::shared_ptr<set<Index> > replacedKeys = recalculate(markedKeys, empty);
-#endif
 
   tic(9,"solve");
   // 9. Solve