diff --git a/examples/Makefile.am b/examples/Makefile.am
index 592241333..3178a55bd 100644
--- a/examples/Makefile.am
+++ b/examples/Makefile.am
@@ -18,7 +18,7 @@ noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial
 noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
 #noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
 #noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
-SUBDIRS = vSLAMexample # does not compile....
+#SUBDIRS = vSLAMexample # does not compile....
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/examples/vSLAMexample/ISAMLoop-inl.h b/examples/vSLAMexample/ISAMLoop-inl.h
index 17e76070c..b78079d70 100644
--- a/examples/vSLAMexample/ISAMLoop-inl.h
+++ b/examples/vSLAMexample/ISAMLoop-inl.h
@@ -8,10 +8,8 @@
 #pragma once
 
 #include <gtsam/linear/GaussianFactorGraph.h>
-//#include <BayesTree-inl.h>
 #include <gtsam/inference/ISAM-inl.h>
 #include <gtsam/nonlinear/Ordering.h>
-//#include <gtsam/inference/IndexTable.h>
 #include <boost/foreach.hpp>
 
 #include "ISAMLoop.h"
@@ -21,39 +19,32 @@ using namespace gtsam;
 /* ************************************************************************* */
 template<class Values>
 void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialValues) {
-  // Reorder and relinearize every reorderInterval updates
+
   if(newFactors.size() > 0) {
-    if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
-      reorder_relinearize();
-      reorderCounter_ = 0;
-    }
 
-    factors_.push_back(newFactors);
+      // Reorder and relinearize every reorderInterval updates
+      if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
+        reorder_relinearize();
+        reorderCounter_ = 0;
+      }
 
-//    BOOST_FOREACH(typename Factors::sharedFactor f, newFactors) {
-//      f->print("Adding factor: ");
-//    }
+      factors_.push_back(newFactors);
 
-    // Linearize new factors and insert them
-    // TODO: optimize for whole config?
-    linPoint_.insert(initialValues);
+      // Linearize new factors and insert them
+      // TODO: optimize for whole config?
+      linPoint_.insert(initialValues);
 
-    // Augment ordering
-    BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
-        BOOST_FOREACH(const Symbol& key, factor->keys()) {
-          ordering_.tryInsert(key, ordering_.nVars());
-        }
-    }
+      // Augment ordering
+      BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
+          BOOST_FOREACH(const Symbol& key, factor->keys()) {
+            ordering_.tryInsert(key, ordering_.nVars());
+          }
+      }
 
-    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
+      boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
 
-    cout << "After linearize: " << endl;
-    BOOST_FOREACH(GaussianFactorGraph::sharedFactor f, *linearizedNewFactors) {
-      f->print("Linearized factor: ");
-    }
-
-    cout << "Update ISAM: " << endl;
-    isam.update(*linearizedNewFactors);
+      // Update ISAM
+      isam.update(*linearizedNewFactors);
   }
 }
 
@@ -61,8 +52,6 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
 template<class Values>
 void ISAMLoop<Values>::reorder_relinearize() {
 
-  //cout << "Reordering " << reorderCounter_;
-
   cout << "Reordering, relinearizing..." << endl;
 
   // Obtain the new linearization point
@@ -73,42 +62,19 @@ void ISAMLoop<Values>::reorder_relinearize() {
   // Compute an ordering
   ordering_ = *factors_.orderingCOLAMD(newLinPoint);
 
-//  cout << "Got estimate" << endl;
-//  newLinPoint.print("newLinPoint");
-//  factors_.print("factors");
-
   // Create a linear factor graph at the new linearization point
   boost::shared_ptr<GaussianFactorGraph> gfg(factors_.linearize(newLinPoint, ordering_));
 
   // Just recreate the whole BayesTree
   isam.update(*gfg);
 
-  //cout << "Reeliminating..." << endl;
-
-//  // Eliminate linear factor graph to a BayesNet with colamd ordering
-//  Ordering ordering = gfg->getOrdering();
-//  const BayesNet<GaussianConditional> bn(
-//      eliminate<GaussianFactor, GaussianConditional>(*gfg, ordering));
-//
-////  cout << "Rebuilding BayesTree..." << endl;
-//
-//  // Replace the BayesTree with a new one
-//  isam.clear();
-//  BOOST_REVERSE_FOREACH(const GaussianISAM::sharedConditional c, bn) {
-//    isam.insert(c, ordering);
-//  }
-
+  // Update linearization point
   linPoint_ = newLinPoint;
-
-//  cout << "Done!" << endl;
 }
 
 /* ************************************************************************* */
 template<class Values>
 Values ISAMLoop<Values>::estimate() {
-//  cout << "ISAMLoop::estimate(): " << endl;
-//  linPoint_.print("linPoint_");
-//  isam.print("isam");
   if(isam.size() > 0)
     return linPoint_.expmap(optimize(isam), ordering_);
   else
diff --git a/examples/vSLAMexample/ISAMLoop.h b/examples/vSLAMexample/ISAMLoop.h
index 424dfdeea..966bd68e5 100644
--- a/examples/vSLAMexample/ISAMLoop.h
+++ b/examples/vSLAMexample/ISAMLoop.h
@@ -50,7 +50,6 @@ public:
 
   /** Return the current solution estimate */
   Values estimate();
-  Values calculateEstimate() { return estimate(); }
 
   /** Return the current linearization point */
   const Values& getLinearizationPoint() { return linPoint_; }
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index fb3fe519b..dfdf74e92 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -41,8 +41,8 @@ string g_dataFolder;
 // Store groundtruth values, read from files
 shared_ptrK g_calib;
 map<int, Point3> g_landmarks;        // map: <landmark_id, landmark_position>
-std::vector<Pose3> g_poses;            // map: <camera_id, pose>
-std::vector<std::vector<Feature2D> > g_measurements;    // map: <camera_id, detected_features> -- where: Feature2D: {camera_id, landmark_id, 2d feature_position}
+std::vector<Pose3> g_poses;          // prior camera poses at each frame
+std::vector<std::vector<Feature2D> > g_measurements;    // feature sets detected at each frame
 
 // Noise models
 SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
@@ -52,13 +52,13 @@ SharedGaussian poseSigma(noiseModel::Unit::Create(1));
 /* ************************************************************************* */
 /**
   * Read all data: calibration file, landmarks, poses, and all features measurements
-  * Data is stored in global variables.
+  * Data is stored in global variables, which are used later to simulate incremental updates.
   */
-void readAllData()
+void readAllDataISAM()
 {
     g_calib = readCalibData(g_dataFolder + CALIB_FILE);
 
-    // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
+    // Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
     g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
 
     // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
@@ -70,9 +70,7 @@ void readAllData()
 
 /* ************************************************************************* */
 /**
-  * Setup vSLAM graph
-  * by adding and linking 2D features (measurements) detected in each captured image
-  * with their corresponding landmarks.
+  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
   */
 void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
                       int pose_id, Pose3& pose,
@@ -119,24 +117,23 @@ int main(int argc, char* argv[])
         exit(0);
     }
 
-    g_dataFolder = string(argv[1]);
-    g_dataFolder += "/";
-    readAllData();
+    g_dataFolder = string(argv[1]) + "/";
+    readAllDataISAM();
 
-    ISAMLoop<Values> isam(3);
+    // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
+    int relinearizeInterval = 3;
+    ISAMLoop<Values> isam(relinearizeInterval);
 
+    // At each frame i with new camera pose and new set of measurements associated with it,
+    // create a graph of new factors and update ISAM
     for (size_t i = 0; i<g_measurements.size(); i++)
-    {
+    { 
         shared_ptr<Graph> newFactors;
         shared_ptr<Values> initialValues;
         createNewFactors(newFactors, initialValues,
                          i, g_poses[i],
                          g_measurements[i], measurementSigma, g_calib);
 
-//        cout << "Add prior pose and measurements of camera " << i << endl;
-//        newFactors->print();
-//        initialValues->print();
-
         isam.update(*newFactors, *initialValues);
         Values currentEstimate = isam.estimate();
         cout << "****************************************************" << endl;
diff --git a/inference/ISAM-inl.h b/inference/ISAM-inl.h
index 73a8185b4..53c71f912 100644
--- a/inference/ISAM-inl.h
+++ b/inference/ISAM-inl.h
@@ -28,51 +28,51 @@ using namespace boost::assign;
 
 namespace gtsam {
 
-	using namespace std;
+  using namespace std;
 
-	/** Create an empty Bayes Tree */
-	template<class CONDITIONAL>
-	ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
+  /** Create an empty Bayes Tree */
+  template<class CONDITIONAL>
+  ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
 
-	/** Create a Bayes Tree from a Bayes Net */
-	template<class CONDITIONAL>
-	ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
-	  BayesTree<CONDITIONAL>(bayesNet) {}
+  /** Create a Bayes Tree from a Bayes Net */
+  template<class CONDITIONAL>
+  ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
+    BayesTree<CONDITIONAL>(bayesNet) {}
 
-	/* ************************************************************************* */
-	template<class CONDITIONAL>
-	template<class FACTORGRAPH>
-	void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
+  /* ************************************************************************* */
+  template<class CONDITIONAL>
+  template<class FACTORGRAPH>
+  void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
 
-		// Remove the contaminated part of the Bayes tree
-		BayesNet<CONDITIONAL> bn;
-		removeTop(newFactors.keys(), bn, orphans);
-		FACTORGRAPH factors(bn);
+    // Remove the contaminated part of the Bayes tree
+    BayesNet<CONDITIONAL> bn;
+    removeTop(newFactors.keys(), bn, orphans);
+    FACTORGRAPH factors(bn);
 
-		// add the factors themselves
-		factors.push_back(newFactors);
+    // add the factors themselves
+    factors.push_back(newFactors);
 
-		// eliminate into a Bayes net
-		typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
+    // eliminate into a Bayes net
+    typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
 
-		// insert conditionals back in, straight into the topless bayesTree
-		typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
-		for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
-			this->insert(*rit);
+    // insert conditionals back in, straight into the topless bayesTree
+    typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
+    for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
+      this->insert(*rit);
 
-		// add orphans to the bottom of the new tree
-		BOOST_FOREACH(sharedClique orphan, orphans) {
-		  this->insert(orphan);
-		}
+    // add orphans to the bottom of the new tree
+    BOOST_FOREACH(sharedClique orphan, orphans) {
+      this->insert(orphan);
+    }
 
-	}
+  }
 
-	template<class CONDITIONAL>
-	template<class FACTORGRAPH>
-	void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
-		Cliques orphans;
-		this->update_internal(newFactors, orphans);
-	}
+  template<class CONDITIONAL>
+  template<class FACTORGRAPH>
+  void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
+    Cliques orphans;
+    this->update_internal(newFactors, orphans);
+  }
 
 }
 /// namespace gtsam
diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h
index b32c559ba..77ddb7f16 100644
--- a/inference/JunctionTree-inl.h
+++ b/inference/JunctionTree-inl.h
@@ -25,6 +25,7 @@
 #include <gtsam/inference/inference-inl.h>
 #include <gtsam/inference/VariableSlots-inl.h>
 #include <gtsam/inference/SymbolicSequentialSolver.h>
+#include <gtsam/inference/ClusterTree-inl.h>
 
 #include <boost/foreach.hpp>
 #include <boost/pool/pool_alloc.hpp>