diff --git a/.cproject b/.cproject
index 35f7946a4..857a2d913 100644
--- a/.cproject
+++ b/.cproject
@@ -21,7 +21,7 @@
 					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
-							<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="6" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
 								<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@@ -102,7 +102,9 @@
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 		<project id="gtsam.null.1344331286" name="gtsam"/>
 	</storageModule>
-	<storageModule moduleId="refreshScope"/>
+	<storageModule moduleId="refreshScope" versionNumber="1">
+		<resource resourceType="PROJECT" workspacePath="/gtsam"/>
+	</storageModule>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
 		<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
diff --git a/.project b/.project
index d20fd358e..ceef32086 100644
--- a/.project
+++ b/.project
@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j2</value>
+					<value>-j6</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
diff --git a/gtsam/base/BTree.h b/gtsam/base/BTree.h
index e0bb4c4a3..2a40465a3 100644
--- a/gtsam/base/BTree.h
+++ b/gtsam/base/BTree.h
@@ -147,7 +147,7 @@ namespace gtsam {
 
 		/** add a key-value pair */
 		BTree add(const KEY& x, const VALUE& d) const {
-			return add(make_pair(x, d));
+			return add(std::make_pair(x, d));
 		}
 
 		/** member predicate */
diff --git a/gtsam/base/DSF.h b/gtsam/base/DSF.h
index ddc5b4836..9952f2e10 100644
--- a/gtsam/base/DSF.h
+++ b/gtsam/base/DSF.h
@@ -59,7 +59,7 @@ namespace gtsam {
 		DSF(const std::set<KEY>& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); }
 
 		// create a new singleton, does nothing if already exists
-		Self makeSet(const KEY& key) const { if (mem(key)) return *this; else return this->add(key, key); }
+		Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); }
 
 		// find the label of the set in which {key} lives
 		Label findSet(const KEY& key) const {
diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h
index df9e74717..7ff8fc076 100644
--- a/gtsam/inference/BayesNet-inl.h
+++ b/gtsam/inference/BayesNet-inl.h
@@ -18,7 +18,6 @@
 #pragma once
 
 #include <gtsam/base/Testable.h>
-#include <gtsam/inference/BayesNet.h>
 
 #include <iostream>
 #include <fstream>
diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h
index 7b9a85122..672d9a4e8 100644
--- a/gtsam/inference/BayesNet.h
+++ b/gtsam/inference/BayesNet.h
@@ -206,4 +206,6 @@ private:
   }
 }; // BayesNet
 
-} /// namespace gtsam
+} // namespace gtsam
+
+#include <gtsam/inference/BayesNet-inl.h>
diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h
index c003102e4..7f77fa898 100644
--- a/gtsam/inference/BayesTree-inl.h
+++ b/gtsam/inference/BayesTree-inl.h
@@ -24,7 +24,7 @@
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/inference.h>
-#include <gtsam/inference/GenericSequentialSolver-inl.h>
+#include <gtsam/inference/GenericSequentialSolver.h>
 
 #include <iostream>
 #include <algorithm>
@@ -501,7 +501,7 @@ namespace gtsam {
 			this->removeClique(clique);
 
 			// remove path above me
-			this->removePath(typename BayesTreeClique<CONDITIONAL>::shared_ptr(clique->parent_.lock()), bn, orphans);
+			this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
 
 			// add children to list of orphans (splice also removed them from clique->children_)
 			orphans.splice (orphans.begin(), clique->children_);
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 8cb9e3278..9106949b1 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -52,7 +52,7 @@ namespace gtsam {
 
 	public:
 
-	  typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
+	  typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
 		typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
 		typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
 		typedef CONDITIONAL ConditionalType;
@@ -178,11 +178,23 @@ namespace gtsam {
 		/** check equality */
 		bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
 
-		/** deep copy from another tree */
 		void cloneTo(shared_ptr& newTree) const {
-		  root_->cloneToBayesTree(*newTree);
+		  cloneTo(newTree, root());
 		}
 
+	private:
+		/** deep copy from another tree */
+		void cloneTo(shared_ptr& newTree, const sharedClique& root) const {
+		  if(root) {
+		    sharedClique newClique = root->clone();
+		    newTree->insert(newClique);
+		    BOOST_FOREACH(const sharedClique& childClique, root->children()) {
+		      cloneTo(newTree, childClique);
+		    }
+		  }
+		}
+	public:
+
 		/**
 		 * Find parent clique of a conditional.  It will look at all parents and
 		 * return the one with the lowest index in the ordering.
@@ -315,14 +327,16 @@ namespace gtsam {
    * extra data along with the clique.
    */
   template<class CONDITIONAL>
-  struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL> > {
+  struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
   public:
     typedef CONDITIONAL ConditionalType;
     typedef BayesTreeClique<CONDITIONAL> This;
-    typedef BayesTreeCliqueBase<This> Base;
+    typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
     typedef boost::shared_ptr<This> shared_ptr;
+    typedef boost::weak_ptr<This> weak_ptr;
     BayesTreeClique() {}
-    BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {}
+    BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
+    BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
 
   private:
     /** Serialization function */
@@ -333,6 +347,7 @@ namespace gtsam {
     }
   };
 
-#include <gtsam/inference/BayesTree-inl.h>
-
 } /// namespace gtsam
+
+#include <gtsam/inference/BayesTree-inl.h>
+#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h
index 2956a6cc2..d2d2cec65 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inl.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inl.h
@@ -17,11 +17,13 @@
 
 #pragma once
 
+#include <gtsam/inference/GenericSequentialSolver.h>
+
 namespace gtsam {
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
+  template<class DERIVED, class CONDITIONAL>
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::assertInvariants() const {
 #ifndef NDEBUG
     // We rely on the keys being sorted
 //    FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
@@ -33,52 +35,60 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
+  template<class DERIVED, class CONDITIONAL>
+  BayesTreeCliqueBase<DERIVED,CONDITIONAL>::BayesTreeCliqueBase(const sharedConditional& conditional) :
+  conditional_(conditional) {
     assertInvariants();
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
+  template<class DERIVED, class CONDITIONAL>
+  BayesTreeCliqueBase<DERIVED,CONDITIONAL>::BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
+  conditional_(result.first) {
+    assertInvariants();
+  }
+
+  /* ************************************************************************* */
+  template<class DERIVED, class CONDITIONAL>
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s) const {
     conditional_->print(s);
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
+  template<class DERIVED, class CONDITIONAL>
+  size_t BayesTreeCliqueBase<DERIVED,CONDITIONAL>::treeSize() const {
     size_t size = 1;
-    BOOST_FOREACH(const shared_ptr& child, children_)
+    BOOST_FOREACH(const derived_ptr& child, children_)
       size += child->treeSize();
     return size;
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
+  template<class DERIVED, class CONDITIONAL>
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::printTree(const std::string& indent) const {
     print(indent);
-    BOOST_FOREACH(const shared_ptr& child, children_)
+    BOOST_FOREACH(const derived_ptr& child, children_)
       child->printTree(indent+"  ");
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
+  template<class DERIVED, class CONDITIONAL>
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
     conditional_->permuteWithInverse(inversePermutation);
-    BOOST_FOREACH(const shared_ptr& child, children_) {
+    BOOST_FOREACH(const derived_ptr& child, children_) {
       child->permuteWithInverse(inversePermutation);
     }
     assertInvariants();
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+  template<class DERIVED, class CONDITIONAL>
+  bool BayesTreeCliqueBase<DERIVED,CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
     bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
 #ifndef NDEBUG
     if(!changed) {
       BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); }
-      BOOST_FOREACH(const shared_ptr& child, children_) {
+      BOOST_FOREACH(const derived_ptr& child, children_) {
         assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
       }
     }
@@ -97,9 +107,8 @@ namespace gtsam {
   // clique on the root. We can compute it recursively from the parent shortcut
   // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  BayesNet<CONDITIONAL> BayesTreeClique<CONDITIONAL>::shortcut(shared_ptr R,
-      Eliminate function) {
+  template<class DERIVED, class CONDITIONAL>
+  BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::shortcut(derived_ptr R, Eliminate function) {
 
     static const bool debug = false;
 
@@ -109,16 +118,16 @@ namespace gtsam {
     shared_ptr parent(parent_.lock());
 
     if (R.get()==this || parent==R) {
-      BayesNet<CONDITIONAL> empty;
+      BayesNet<ConditionalType> empty;
       return empty;
     }
 
     // The root conditional
-    FactorGraph<FactorType> p_R(BayesNet<CONDITIONAL>(R->conditional()));
+    FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional()));
 
-    // The parent clique has a CONDITIONAL for each frontal node in Fp
+    // The parent clique has a ConditionalType for each frontal node in Fp
     // so we can obtain P(Fp|Sp) in factor graph form
-    FactorGraph<FactorType> p_Fp_Sp(BayesNet<CONDITIONAL>(parent->conditional()));
+    FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(parent->conditional()));
 
     // If not the base case, obtain the parent shortcut P(Sp|R) as factors
     FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
@@ -151,29 +160,29 @@ namespace gtsam {
     BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
       variablesAtBack.insert(separatorIndex);
       separator.insert(separatorIndex);
-      if(debug) cout << "At back (this): " << separatorIndex << endl;
+      if(debug) std::cout << "At back (this): " << separatorIndex << std::endl;
     }
     BOOST_FOREACH(const Index key, R->conditional()->keys()) {
       if(variablesAtBack.insert(key).second)
         ++ uniqueRootVariables;
-      if(debug) cout << "At back (root): " << key << endl;
+      if(debug) std::cout << "At back (root): " << key << std::endl;
     }
 
     Permutation toBack = Permutation::PushToBack(
-        vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
+        std::vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
         R->conditional()->lastFrontalKey() + 1);
     Permutation::shared_ptr toBackInverse(toBack.inverse());
     BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
       factor->permuteWithInverse(*toBackInverse); }
-    typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<
+    typename BayesNet<ConditionalType>::shared_ptr eliminated(EliminationTree<
         FactorType>::Create(p_Cp_R)->eliminate(function));
 
     // Take only the conditionals for p(S|R).  We check for each variable being
     // in the separator set because if some separator variables overlap with
     // root variables, we cannot rely on the number of root variables, and also
     // want to include those variables in the conditional.
-    BayesNet<CONDITIONAL> p_S_R;
-    BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) {
+    BayesNet<ConditionalType> p_S_R;
+    BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) {
       assert(conditional->nrFrontals() == 1);
       if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
         if(debug)
@@ -199,16 +208,16 @@ namespace gtsam {
   // \int(Cp\S) P(F|S)P(S|Cp)P(Cp)
   // Because the root clique could be very big.
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  FactorGraph<typename CONDITIONAL::FactorType> BayesTreeClique<CONDITIONAL>::marginal(
-      shared_ptr R, Eliminate function) {
+  template<class DERIVED, class CONDITIONAL>
+  FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::marginal(
+      derived_ptr R, Eliminate function) {
     // If we are the root, just return this root
     // NOTE: immediately cast to a factor graph
-    BayesNet<CONDITIONAL> bn(R->conditional());
+    BayesNet<ConditionalType> bn(R->conditional());
     if (R.get()==this) return bn;
 
     // Combine P(F|S), P(S|R), and P(R)
-    BayesNet<CONDITIONAL> p_FSR = this->shortcut(R, function);
+    BayesNet<ConditionalType> p_FSR = this->shortcut(R, function);
     p_FSR.push_front(this->conditional());
     p_FSR.push_back(R->conditional());
 
@@ -220,9 +229,9 @@ namespace gtsam {
   /* ************************************************************************* */
   // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
   /* ************************************************************************* */
-  template<class CONDITIONAL>
-  FactorGraph<typename CONDITIONAL::FactorType> BayesTreeClique<CONDITIONAL>::joint(
-      shared_ptr C2, shared_ptr R, Eliminate function) {
+  template<class DERIVED, class CONDITIONAL>
+  FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::joint(
+      derived_ptr C2, derived_ptr R, Eliminate function) {
     // For now, assume neither is the root
 
     // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
@@ -234,14 +243,14 @@ namespace gtsam {
     joint.push_back(R->conditional()->toFactor()); // P(R)
 
     // Find the keys of both C1 and C2
-    vector<Index> keys1(conditional_->keys());
-    vector<Index> keys2(C2->conditional_->keys());
+    std::vector<Index> keys1(conditional_->keys());
+    std::vector<Index> keys2(C2->conditional_->keys());
     FastSet<Index> keys12;
     keys12.insert(keys1.begin(), keys1.end());
     keys12.insert(keys2.begin(), keys2.end());
 
     // Calculate the marginal
-    vector<Index> keys12vector; keys12vector.reserve(keys12.size());
+    std::vector<Index> keys12vector; keys12vector.reserve(keys12.size());
     keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
     assertInvariants();
     GenericSequentialSolver<FactorType> solver(joint);
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 83496dfc0..fe7c1b81b 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -24,6 +24,8 @@
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/BayesNet.h>
 
+namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTree; }
+
 namespace gtsam {
 
   /**
@@ -39,9 +41,21 @@ namespace gtsam {
    *
    * @tparam The derived clique type.
    */
-  template<class DERIVED>
+  template<class DERIVED, class CONDITIONAL>
   struct BayesTreeCliqueBase {
 
+  public:
+    typedef BayesTreeCliqueBase<DERIVED,CONDITIONAL> This;
+    typedef DERIVED DerivedType;
+    typedef CONDITIONAL ConditionalType;
+    typedef boost::shared_ptr<ConditionalType> sharedConditional;
+    typedef boost::shared_ptr<This> shared_ptr;
+    typedef boost::weak_ptr<This> weak_ptr;
+    typedef boost::shared_ptr<DerivedType> derived_ptr;
+    typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
+    typedef typename ConditionalType::FactorType FactorType;
+    typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
+
   protected:
     void assertInvariants() const;
 
@@ -51,18 +65,10 @@ namespace gtsam {
     /** Construct from a conditional, leaving parent and child pointers uninitialized */
     BayesTreeCliqueBase(const sharedConditional& conditional);
 
-  public:
-    typedef BayesTreeClique<DERIVED> This;
-    typedef DERIVED DerivedType;
-    typedef typename DERIVED::ConditionalType ConditionalType;
-    typedef boost::shared_ptr<ConditionalType> sharedConditional;
-    typedef typename boost::shared_ptr<This> shared_ptr;
-    typedef typename boost::weak_ptr<This> weak_ptr;
-    typedef typename boost::shared_ptr<DerivedType> derived_ptr;
-    typedef typename boost::weak_ptr<DerivedType> derived_weak_ptr;
-    typedef typename ConditionalType::FactorType FactorType;
-    typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
+    /** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
+    BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result);
 
+  public:
     sharedConditional conditional_;
     derived_weak_ptr parent_;
     std::list<derived_ptr> children_;
@@ -77,19 +83,7 @@ namespace gtsam {
      */
     static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
 
-    void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const {
-      sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_));
-      sharedClique newClique = newTree.addClique(newConditional, parent_clique);
-      if (cachedFactor_)
-        newClique->cachedFactor_ = cachedFactor_->clone();
-      else newClique->cachedFactor_ = typename FactorType::shared_ptr();
-      if (!parent_clique) {
-        newTree.root_ = newClique;
-      }
-      BOOST_FOREACH(const shared_ptr& childClique, children_) {
-        childClique->cloneToBayesTree(newTree, newClique);
-      }
-    }
+    derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
 
     /** print this node */
     void print(const std::string& s = "") const;
@@ -128,19 +122,21 @@ namespace gtsam {
 
     /** return the conditional P(S|Root) on the separator given the root */
     // TODO: create a cached version
-    BayesNet<ConditionalType> shortcut(shared_ptr root, Eliminate function);
+    BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function);
 
     /** return the marginal P(C) of the clique */
-    FactorGraph<FactorType> marginal(shared_ptr root, Eliminate function);
+    FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function);
 
     /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
-    FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
+    FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function);
 
     bool equals(const This& other, double tol=1e-9) const {
       return (!conditional_ && !other.conditional()) ||
           conditional_->equals(*(other.conditional()), tol);
     }
 
+    friend class BayesTree<ConditionalType, DerivedType>;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
@@ -153,8 +149,8 @@ namespace gtsam {
 
   }; // \struct Clique
 
-  template<class DERIVED>
-  typename BayesTreeCliqueBase<DERIVED>::derived_ptr asDerived(const BayesTreeCliqueBase<DERIVED>& base) {
+  template<class DERIVED, class CONDITIONAL>
+  typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::derived_ptr asDerived(const BayesTreeCliqueBase<DERIVED,CONDITIONAL>& base) {
 #ifndef NDEBUG
     return boost::dynamic_pointer_cast<DERIVED>(base);
 #else
@@ -162,6 +158,4 @@ namespace gtsam {
 #endif
   }
 
-#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
-
 }
diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h
index 1d62e46fc..462a2fd4a 100644
--- a/gtsam/inference/EliminationTree-inl.h
+++ b/gtsam/inference/EliminationTree-inl.h
@@ -9,7 +9,6 @@
 #include <gtsam/base/FastSet.h>
 #include <gtsam/inference/EliminationTree.h>
 #include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/FactorGraph-inl.h>
 #include <gtsam/inference/IndexFactor.h>
 #include <gtsam/inference/IndexConditional.h>
 
diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h
index 6e47983a0..76db131ed 100644
--- a/gtsam/inference/EliminationTree.h
+++ b/gtsam/inference/EliminationTree.h
@@ -10,10 +10,10 @@
 #include <gtsam/base/FastSet.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/VariableIndex.h>
-#include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/FactorGraph.h>
 
 class EliminationTreeTester; // for unit tests, see testEliminationTree
+namespace gtsam { template<class CONDITIONAL> class BayesNet; }
 
 namespace gtsam {
 
@@ -140,3 +140,5 @@ struct DisconnectedGraphException : public std::exception {
 };
 
 }
+
+#include <gtsam/inference/EliminationTree-inl.h>
diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index e776a4390..3b3d568bd 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -24,7 +24,7 @@
 
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/graph-inl.h>
-#include <gtsam/inference/BayesTree-inl.h>
+#include <gtsam/inference/BayesTree.h>
 #include <gtsam/base/DSF.h>
 
 #include <boost/foreach.hpp>
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 645b496ec..3453f14a6 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -239,3 +239,4 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 
 } // namespace gtsam
 
+#include <gtsam/inference/FactorGraph-inl.h>
diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h
index b4bd22e67..04e8cfbeb 100644
--- a/gtsam/inference/GenericSequentialSolver-inl.h
+++ b/gtsam/inference/GenericSequentialSolver-inl.h
@@ -18,10 +18,10 @@
 
 #pragma once
 
-#include <gtsam/inference/GenericSequentialSolver.h>
-#include <gtsam/inference/Factor-inl.h>
-#include <gtsam/inference/EliminationTree-inl.h>
-#include <gtsam/inference/BayesNet-inl.h>
+#include <gtsam/inference/Factor.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/EliminationTree.h>
+#include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/inference.h>
 
 #include <boost/foreach.hpp>
@@ -77,9 +77,8 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class FACTOR>
-	typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
-	GenericSequentialSolver<FACTOR>::eliminate(
-			typename EliminationTree<FACTOR>::Eliminate function) const {
+	typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > //
+	GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
 		return eliminationTree_->eliminate(function);
 	}
 
@@ -87,8 +86,7 @@ namespace gtsam {
 	template<class FACTOR>
 	typename FactorGraph<FACTOR>::shared_ptr //
 	GenericSequentialSolver<FACTOR>::jointFactorGraph(
-			const std::vector<Index>& js,
-			typename EliminationTree<FACTOR>::Eliminate function) const {
+			const std::vector<Index>& js, Eliminate function) const {
 
 		// Compute a COLAMD permutation with the marginal variable constrained to the end.
 		Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
@@ -127,10 +125,9 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class FACTOR>
 	typename FACTOR::shared_ptr //
-	GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
-			typename EliminationTree<FACTOR>::Eliminate function) const {
+	GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
 		// Create a container for the one variable index
-		vector<Index> js(1);
+		std::vector<Index> js(1);
 		js[0] = j;
 
 		// Call joint and return the only factor in the factor graph it returns
diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h
index 16d47f8df..d9c593b25 100644
--- a/gtsam/inference/GenericSequentialSolver.h
+++ b/gtsam/inference/GenericSequentialSolver.h
@@ -19,9 +19,15 @@
 #pragma once
 
 #include <utility>
-
+#include <boost/function.hpp>
+#include <vector>
+#include <gtsam/base/types.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/inference/EliminationTree.h>
+
+namespace gtsam { class VariableIndex; }
+namespace gtsam { template<class FACTOR> class EliminationTree; }
+namespace gtsam { template<class FACTOR> class FactorGraph; }
+namespace gtsam { template<class CONDITIONAL> class BayesNet; }
 
 namespace gtsam {
 
@@ -43,7 +49,12 @@ namespace gtsam {
 
 	protected:
 
-		typedef typename FactorGraph<FACTOR>::shared_ptr sharedFactorGraph;
+		typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
+
+		typedef std::pair<
+		        boost::shared_ptr<typename FACTOR::ConditionalType>,
+		        boost::shared_ptr<FACTOR> > EliminationResult;
+		typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
 
 		/** Store the original factors for computing marginals
 		 * TODO Frank says: really? Marginals should be computed from result.
@@ -51,15 +62,14 @@ namespace gtsam {
 		sharedFactorGraph factors_;
 
 		/** Store column structure of the factor graph. Why? */
-		VariableIndex::shared_ptr structure_;
+		boost::shared_ptr<VariableIndex> structure_;
 
 		/** Elimination tree that performs elimination */
-		typedef EliminationTree<FACTOR> EliminationTree_;
-		typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;
+		boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
 
 		/** concept checks */
 		GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
-		GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree_)
+//		GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
 
 	public:
 
@@ -76,7 +86,7 @@ namespace gtsam {
 		 */
 		GenericSequentialSolver(
 				const sharedFactorGraph& factorGraph,
-				const VariableIndex::shared_ptr& variableIndex);
+				const boost::shared_ptr<VariableIndex>& variableIndex);
 
 		/** Print to cout */
 		void print(const std::string& name = "GenericSequentialSolver: ") const;
@@ -95,25 +105,23 @@ namespace gtsam {
 		 * Eliminate the factor graph sequentially.  Uses a column elimination tree
 		 * to recursively eliminate.
 		 */
-		typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr
-		eliminate(typename EliminationTree<FACTOR>::Eliminate function) const;
+		typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > eliminate(Eliminate function) const;
 
 		/**
 		 * Compute the marginal joint over a set of variables, by integrating out
 		 * all of the other variables.  Returns the result as a factor graph.
 		 */
 		typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(
-				const std::vector<Index>& js,
-				typename EliminationTree<FACTOR>::Eliminate function) const;
+				const std::vector<Index>& js, Eliminate function) const;
 
 		/**
 		 * Compute the marginal Gaussian density over a variable, by integrating out
 		 * all of the other variables.  This function returns the result as a factor.
 		 */
-		typename FACTOR::shared_ptr marginalFactor(Index j,
-				typename EliminationTree<FACTOR>::Eliminate function) const;
+		typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
 
 	}; // GenericSequentialSolver
 
 } // namespace gtsam
 
+#include <gtsam/inference/GenericSequentialSolver-inl.h>
diff --git a/gtsam/inference/ISAM-inl.h b/gtsam/inference/ISAM-inl.h
index 8fdad31b2..1daab686f 100644
--- a/gtsam/inference/ISAM-inl.h
+++ b/gtsam/inference/ISAM-inl.h
@@ -41,7 +41,7 @@ namespace gtsam {
 
     // Remove the contaminated part of the Bayes tree
     BayesNet<CONDITIONAL> bn;
-    removeTop(newFactors.keys(), bn, orphans);
+    this->removeTop(newFactors.keys(), bn, orphans);
     FG factors(bn);
 
     // add the factors themselves
diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h
index b3f5edd49..fca4e2a27 100644
--- a/gtsam/inference/JunctionTree-inl.h
+++ b/gtsam/inference/JunctionTree-inl.h
@@ -21,11 +21,9 @@
 
 #include <gtsam/base/timing.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
-#include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/EliminationTree-inl.h>
-#include <gtsam/inference/ClusterTree-inl.h>
+#include <gtsam/inference/EliminationTree.h>
 
 #include <boost/foreach.hpp>
 #include <boost/lambda/bind.hpp>
diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h
index 12c7dd5d2..2f74357fe 100644
--- a/gtsam/inference/JunctionTree.h
+++ b/gtsam/inference/JunctionTree.h
@@ -110,3 +110,5 @@ namespace gtsam {
 	}; // JunctionTree
 
 } // namespace gtsam
+
+#include <gtsam/inference/JunctionTree-inl.h>
diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp
index 5a008a192..2d5190f45 100644
--- a/gtsam/inference/SymbolicFactorGraph.cpp
+++ b/gtsam/inference/SymbolicFactorGraph.cpp
@@ -18,19 +18,14 @@
 #include <boost/make_shared.hpp>
 
 #include <gtsam/inference/SymbolicFactorGraph.h>
-#include <gtsam/inference/FactorGraph-inl.h>
-#include <gtsam/inference/BayesNet-inl.h>
-#include <gtsam/inference/EliminationTree-inl.h>
+#include <gtsam/inference/BayesNet.h>
+#include <gtsam/inference/EliminationTree.h>
+#include <gtsam/inference/IndexConditional.h>
 
 namespace gtsam {
 
 	using namespace std;
 
-	// Explicitly instantiate so we don't have to include everywhere
-	template class FactorGraph<IndexFactor>;
-	template class BayesNet<IndexConditional>;
-	template class EliminationTree<IndexFactor>;
-
   /* ************************************************************************* */
 	SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet) :
 	    FactorGraph<IndexFactor>(bayesNet) {}
diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h
index dd9932d74..90413ab06 100644
--- a/gtsam/inference/SymbolicFactorGraph.h
+++ b/gtsam/inference/SymbolicFactorGraph.h
@@ -17,8 +17,13 @@
 
 #pragma once
 
-#include <gtsam/inference/IndexConditional.h>
-#include <gtsam/inference/EliminationTree.h>
+#include <gtsam/base/types.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/IndexFactor.h>
+
+namespace gtsam { template<class FACTOR> class EliminationTree; }
+namespace gtsam { template<class CONDITIONAL> class BayesNet; }
+namespace gtsam { class IndexConditional; }
 
 namespace gtsam {
 
@@ -72,7 +77,7 @@ namespace gtsam {
 	 * Combine and eliminate can also be called separately, but for this and
 	 * derived classes calling them separately generally does extra work.
 	 */
-	std::pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr>
+	std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
 	EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
 
 	/* Template function implementation */
diff --git a/gtsam/inference/SymbolicMultifrontalSolver.cpp b/gtsam/inference/SymbolicMultifrontalSolver.cpp
index 77df4e04d..7348cbc5d 100644
--- a/gtsam/inference/SymbolicMultifrontalSolver.cpp
+++ b/gtsam/inference/SymbolicMultifrontalSolver.cpp
@@ -16,7 +16,7 @@
  */
 
 #include <gtsam/inference/SymbolicMultifrontalSolver.h>
-#include <gtsam/inference/JunctionTree-inl.h>
+#include <gtsam/inference/JunctionTree.h>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index 2be90a074..d853ff91b 100644
--- a/gtsam/inference/inference.h
+++ b/gtsam/inference/inference.h
@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/Permutation.h>
 
@@ -77,4 +76,4 @@ namespace gtsam {
 	  return PermutationCOLAMD_(variableIndex, cmember);
 	}
 
-} /// namespace gtsam
+} // namespace gtsam
diff --git a/gtsam/inference/tests/testClusterTree.cpp b/gtsam/inference/tests/testClusterTree.cpp
index ab7716c50..7c7e7dc9a 100644
--- a/gtsam/inference/tests/testClusterTree.cpp
+++ b/gtsam/inference/tests/testClusterTree.cpp
@@ -27,7 +27,7 @@ using namespace boost::assign;
 using namespace gtsam;
 
 // explicit instantiation and typedef
-template class ClusterTree<SymbolicFactorGraph>;
+namespace gtsam { template class ClusterTree<SymbolicFactorGraph>; }
 typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree;
 
 /* ************************************************************************* */
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 7dd129472..e35eb8a8f 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -17,6 +17,7 @@
  */
 
 #include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/vector.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
 using namespace boost::assign;
 
@@ -35,6 +36,7 @@ using namespace boost::assign;
 #include <gtsam/inference/SymbolicSequentialSolver.h>
 
 using namespace gtsam;
+using namespace std;
 
 typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
 typedef BayesTree<IndexConditional> SymbolicBayesTree;
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 2bbfc2f22..8a58bbc34 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -24,7 +24,6 @@
 #include <gtsam/base/types.h>
 #include <gtsam/base/blockMatrices.h>
 #include <gtsam/inference/IndexConditional.h>
-#include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 
 // Forward declaration to friend unit tests
@@ -39,6 +38,7 @@ namespace gtsam {
 
 // Forward declarations
 class GaussianFactor;
+class JacobianFactor;
 
 /**
  * A conditional Gaussian functions as the node in a Bayes network
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index eeee0dccc..ba21eed25 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -22,6 +22,8 @@
 
 #include <gtsam/linear/GaussianConditional.h>
 
+namespace gtsam { class SharedDiagonal; }
+
 namespace gtsam {
 
 	class KalmanFilter {
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index d211b2e06..e11765b6c 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/TestableAssertions.h>
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 
diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp
index d873da83c..4ceda0cfb 100644
--- a/gtsam/linear/tests/testKalmanFilter.cpp
+++ b/gtsam/linear/tests/testKalmanFilter.cpp
@@ -18,6 +18,8 @@
  */
 
 #include <gtsam/linear/KalmanFilter.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/linear/SharedDiagonal.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2-inl.h
similarity index 84%
rename from gtsam/nonlinear/GaussianISAM2.cpp
rename to gtsam/nonlinear/GaussianISAM2-inl.h
index 7df21bf2c..7fcd77873 100644
--- a/gtsam/nonlinear/GaussianISAM2.cpp
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -14,7 +14,8 @@ using namespace gtsam;
 namespace gtsam {
 
 /* ************************************************************************* */
-void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, double threshold,
+  template<class CLIQUE>
+void optimize2(const boost::shared_ptr<CLIQUE>& clique, double threshold,
 		vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
 	// if none of the variables in this clique (frontal and separator!) changed
 	// significantly, then by the running intersection property, none of the
@@ -84,7 +85,7 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
     }
 
     // Recurse to children
-    BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
+    BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
       optimize2(child, threshold, changed, replaced, delta, count);
     }
 	}
@@ -92,14 +93,15 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
 
 /* ************************************************************************* */
 // fast full version without threshold
-void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& delta) {
+  template<class CLIQUE>
+void optimize2(const boost::shared_ptr<CLIQUE>& clique, VectorValues& delta) {
 
 	// parents are assumed to already be solved and available in result
   (*clique)->rhs(delta);
   (*clique)->solveInPlace(delta);
 
   // Solve chilren recursively
-  BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
+  BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
 		optimize2(child, delta);
 	}
 }
@@ -114,7 +116,8 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, Vecto
 //}
 
 /* ************************************************************************* */
-int optimize2(const BayesTree<GaussianConditional>::sharedClique& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
+  template<class CLIQUE>
+int optimize2(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
 	vector<bool> changed(keys.size(), false);
 	int count = 0;
 	// starting from the root, call optimize on each conditional
@@ -123,18 +126,20 @@ int optimize2(const BayesTree<GaussianConditional>::sharedClique& root, double t
 }
 
 /* ************************************************************************* */
-void nnz_internal(const BayesTree<GaussianConditional>::sharedClique& clique, int& result) {
+  template<class CLIQUE>
+void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
   int dimR = (*clique)->dim();
   int dimSep = (*clique)->get_S().cols() - dimR;
   result += ((dimR+1)*dimR)/2 + dimSep*dimR;
 	// traverse the children
-	BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
+	BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
 		nnz_internal(child, result);
 	}
 }
 
 /* ************************************************************************* */
-int calculate_nnz(const BayesTree<GaussianConditional>::sharedClique& clique) {
+  template<class CLIQUE>
+int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
 	int result = 0;
 	// starting from the root, add up entries of frontal and conditional matrices of each conditional
 	nnz_internal(clique, result);
diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h
index fe29936cb..cb6f39865 100644
--- a/gtsam/nonlinear/GaussianISAM2.h
+++ b/gtsam/nonlinear/GaussianISAM2.h
@@ -42,7 +42,8 @@ public:
 };
 
 // optimize the BayesTree, starting from the root
-void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorValues& delta);
+template<class CLIQUE>
+void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
 
 // optimize the BayesTree, starting from the root; "replaced" needs to contain
 // all variables that are contained in the top of the Bayes tree that has been
@@ -52,10 +53,14 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorV
 // and recursive backsubstitution might eventually stop if none of the changed
 // variables are contained in the subtree.
 // returns the number of variables that were solved for
-int optimize2(const BayesTree<GaussianConditional>::sharedClique& root,
+template<class CLIQUE>
+int optimize2(const boost::shared_ptr<CLIQUE>& root,
     double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
 
 // calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
-int calculate_nnz(const BayesTree<GaussianConditional>::sharedClique& clique);
+template<class CLIQUE>
+int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
 
 }/// namespace gtsam
+
+#include <gtsam/nonlinear/GaussianISAM2-inl.h>
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index d414e13e9..85b28d744 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -81,7 +81,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    *
    * Alternatively could we trace up towards the root for each variable here?
    */
-  static void FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
+  static void FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
 
   /**
    * Apply expmap to the given values, but only for indices appearing in
@@ -148,7 +148,7 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
   const size_t originalDim = delta->dim();
   const size_t originalnVars = delta->size();
   delta.container().append(dims);
-  delta.container().vector().segment(originalDim, newDim) = Vector::Zero(newDim);
+  delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   delta.permutation().resize(originalnVars + newTheta.size());
   {
     _VariableAdder vadder(ordering, delta, originalnVars);
@@ -188,7 +188,7 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(Permu
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
+void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
   static const bool debug = false;
   // does the separator contain any of the variables?
   bool found = false;
@@ -202,7 +202,7 @@ void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIO
     if(debug) clique->print("Key(s) marked in clique ");
     if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
   }
-  BOOST_FOREACH(const typename BayesTreeClique<CONDITIONAL>::shared_ptr& child, clique->children_) {
+  BOOST_FOREACH(const typename ISAM2Clique<CONDITIONAL>::shared_ptr& child, clique->children_) {
     FindAll(child, keys, markedMask);
   }
 }
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 4ed103615..5d2984c9c 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -286,7 +286,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
     toc(5,"eliminate");
 
     tic(6,"insert");
-    BayesTree<CONDITIONAL>::clear();
+    this->clear();
     this->insert(newRoot);
     toc(6,"insert");
 
@@ -310,6 +310,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
     affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
     tic(1,"relinearizeAffected");
     GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys));
+    if(debug) factors.print("Relinearized factors: ");
     toc(1,"relinearizeAffected");
 
     if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index f3e960db4..8663aae41 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -106,21 +106,18 @@ struct ISAM2Result {
 };
 
 template<class CONDITIONAL>
-struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL> > {
+struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDITIONAL> {
 
   typedef ISAM2Clique<CONDITIONAL> This;
-  typedef BayesTreeCliqueBase<This> Base;
+  typedef BayesTreeCliqueBase<This,CONDITIONAL> Base;
   typedef boost::shared_ptr<This> shared_ptr;
+  typedef boost::weak_ptr<This> weak_ptr;
+  typedef CONDITIONAL ConditionalType;
+  typedef typename ConditionalType::shared_ptr sharedConditional;
 
   typename Base::FactorType::shared_ptr cachedFactor_;
   Vector gradientContribution_;
 
-  /** Access the cached factor */
-  typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
-
-  /** Access the gradient contribution */
-  const Vector& gradientContribution() const { return gradientContribution_; }
-
   /** Construct from a conditional */
   ISAM2Clique(const sharedConditional& conditional) : Base(conditional) {
     throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
@@ -130,8 +127,33 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL> > {
     Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
     // Compute gradient contribution
     const ConditionalType& conditional(*result.first);
-    gradient << -(conditional.get_R() * conditional.permutation().transpose()).transpose * conditional.get_d(),
-        -conditional.get_S() * conditional.get_d();
+    gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(),
+        -conditional.get_S().transpose() * conditional.get_d();
+  }
+
+  /** Produce a deep copy, copying the cached factor and gradient contribution */
+  shared_ptr clone() const {
+    shared_ptr copy(new ISAM2Clique(make_pair(
+        sharedConditional(new ConditionalType(*Base::conditional_)),
+        cachedFactor_ ? cachedFactor_->clone() : typename Base::FactorType::shared_ptr())));
+    copy->gradientContribution_ = gradientContribution_;
+    return copy;
+  }
+
+  /** Access the cached factor */
+  typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
+
+  /** Access the gradient contribution */
+  const Vector& gradientContribution() const { return gradientContribution_; }
+
+  void permuteWithInverse(const Permutation& inversePermutation) {
+    if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+    Base::permuteWithInverse(inversePermutation);
+  }
+
+  bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+    if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+    return Base::permuteSeparatorWithInverse(inversePermutation);
   }
 
 private:
@@ -214,7 +236,7 @@ public:
   typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
   typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
 
-  void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
+  void cloneTo(boost::shared_ptr<This>& newISAM2) const {
     boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
     Base::cloneTo(bayesTree);
     newISAM2->theta_ = theta_;
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 610bb7f5c..b944b105d 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -30,7 +30,7 @@ headers += DoglegOptimizer.h DoglegOptimizer-inl.h
 # Nonlinear iSAM(2)
 headers += NonlinearISAM.h NonlinearISAM-inl.h
 headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h
-sources += GaussianISAM2.cpp
+headers += GaussianISAM2.h GaussianISAM2-inl.h
 
 # Nonlinear constraints 
 headers += NonlinearEquality.h 
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index ad22d3e41..c65b4ac08 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -249,7 +249,7 @@ public:
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
   virtual double error(const VALUES& c) const {
-  	if (active(c))
+  	if (this->active(c))
   		return 0.5 * noiseModel_->distance(unwhitenedError(c));
   	else
   		return 0.0;
@@ -262,7 +262,7 @@ public:
    */
   boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
   	// Only linearize if the factor is active
-		if (!active(x))
+		if (!this->active(x))
 			return boost::shared_ptr<JacobianFactor>();
 
     // Create the set of terms - Jacobians for each index
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 79a9aa436..faa06bbc7 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <map>
+#include <set>
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/inference/inference.h>
 
diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp
index 64f5e7f4c..8c1804016 100644
--- a/gtsam/nonlinear/tests/testOrdering.cpp
+++ b/gtsam/nonlinear/tests/testOrdering.cpp
@@ -15,6 +15,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 using namespace gtsam;
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 121717cf3..8bdacbf0f 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -167,7 +167,8 @@ TEST(ISAM2, optimize2) {
   conditional->solveInPlace(expected);
 
   // Clique
-  GaussianISAM2<planarSLAM::Values>::sharedClique clique(GaussianISAM2<planarSLAM::Values>::Clique::Create(conditional));
+  GaussianISAM2<planarSLAM::Values>::sharedClique clique(
+      GaussianISAM2<planarSLAM::Values>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
   VectorValues actual(theta.dims(ordering));
   conditional->rhs(actual);
   optimize2(clique, actual);
@@ -192,9 +193,13 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu
 }
 
 /* ************************************************************************* */
-TEST(ISAM2, slamlike_solution)
+TEST_UNSAFE(ISAM2, slamlike_solution)
 {
 
+//  SETDEBUG("ISAM2 update", true);
+//  SETDEBUG("ISAM2 update verbose", true);
+//  SETDEBUG("ISAM2 recalculate", true);
+
   // Pose and landmark key types from planarSLAM
   typedef planarSLAM::PoseKey PoseKey;
   typedef planarSLAM::PointKey PointKey;