diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index cb08871d9..18af63006 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -50,14 +50,29 @@ public:
 
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
+  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(first != last)
+      Base::assign(first, last);
+  }
 
   /** Copy constructor from another FastSet */
   FastVector(const FastVector<VALUE>& x) : Base(x) {}
 
-  /** Copy constructor from the base map class */
+  /** Copy constructor from the base class */
   FastVector(const Base& x) : Base(x) {}
 
+  /** Copy constructor from a standard STL container */
+  FastVector(const std::vector<VALUE>& x) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(x.size() > 0)
+      Base::assign(x.begin(), x.end());
+  }
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index bc530a9f5..a770d9975 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
 template <class T>
 T expm(const Vector& x, int K=7) {
 	Matrix xhat = wedge<T>(x);
-	return expm(xhat,K);
+	return T(expm(xhat,K));
 }
 
 } // namespace gtsam
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 87788e3d8..3dc73519a 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -297,7 +297,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONDITIONAL, class CLIQUE>
   void _BayesTree_dim_adder(
-      std::vector<size_t>& dims,
+      FastVector<size_t>& dims,
       const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
 
     if(clique) {
@@ -316,7 +316,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class CONDITIONAL,class CLIQUE>
 	boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
-	  std::vector<size_t> dimensions(bt.nodes().size(), 0);
+	  FastVector<size_t> dimensions(bt.nodes().size(), 0);
 	  _BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
 	  return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
 	}
diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h
index dfc8864f7..1b4d92cae 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inl.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inl.h
@@ -223,7 +223,7 @@ namespace gtsam {
 
     assertInvariants();
     GenericSequentialSolver<FactorType> solver(p_FSR);
-    return *solver.jointFactorGraph(conditional_->keys(), function);
+    return *solver.jointFactorGraph(vector<Index>(conditional_->keys().begin(), conditional_->keys().end()), function);
   }
 
   /* ************************************************************************* */
@@ -243,8 +243,8 @@ namespace gtsam {
     joint.push_back(R->conditional()->toFactor()); // P(R)
 
     // Find the keys of both C1 and C2
-    std::vector<Index> keys1(conditional_->keys());
-    std::vector<Index> keys2(C2->conditional_->keys());
+    const FastVector<Index>& keys1(conditional_->keys());
+    const FastVector<Index>& keys2(C2->conditional_->keys());
     FastSet<Index> keys12;
     keys12.insert(keys1.begin(), keys1.end());
     keys12.insert(keys2.begin(), keys2.end());
diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index 7544587c1..9e82aae18 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -20,12 +20,13 @@
 #pragma once
 
 #include <list>
-#include <vector>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/weak_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/FastList.h>
 
 namespace gtsam {
 
@@ -46,13 +47,13 @@ namespace gtsam {
 			typedef typename boost::shared_ptr<Cluster> shared_ptr;
 			typedef typename boost::weak_ptr<Cluster> weak_ptr;
 
-      const std::vector<Index> frontal;                   // the frontal variables
-      const std::vector<Index> separator;                // the separator variables
+      const FastVector<Index> frontal;                   // the frontal variables
+      const FastVector<Index> separator;                // the separator variables
 
 		protected:
 
 			weak_ptr parent_;                      // the parent cluster
-			std::list<shared_ptr> children_;     // the child clusters
+			FastList<shared_ptr> children_;     // the child clusters
 			const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
 
 		public:
@@ -82,7 +83,7 @@ namespace gtsam {
 			bool equals(const Cluster& other) const;
 
 			/// get a reference to the children
-			const std::list<shared_ptr>& children() const { return children_; }
+			const FastList<shared_ptr>& children() const { return children_; }
 
 			/// add a child
 			void addChild(shared_ptr child);
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index 2fa5f691d..a2f5bd7a1 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -45,8 +45,8 @@ private:
 
 	/** Create keys by adding key in front */
 	template<typename ITERATOR>
-	static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
-		std::vector<Key> keys((lastParent - firstParent) + 1);
+	static FastVector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
+		FastVector<Key> keys((lastParent - firstParent) + 1);
 		std::copy(firstParent, lastParent, keys.begin() + 1);
 		keys[0] = key;
 		return keys;
@@ -116,8 +116,14 @@ public:
 		assertInvariants();
 	}
 
+  /** Constructor from a frontal variable and a vector of parents */
+  Conditional(Key key, const FastVector<Key>& parents) :
+    FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
+    assertInvariants();
+  }
+
   /** Constructor from keys and nr of frontal variables */
-	Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
+	Conditional(const FastVector<Index>& keys, size_t nrFrontals) :
 		FactorType(keys), nrFrontals_(nrFrontals) {
 		assertInvariants();
 	}
diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h
index 462a2fd4a..002a5639c 100644
--- a/gtsam/inference/EliminationTree-inl.h
+++ b/gtsam/inference/EliminationTree-inl.h
@@ -58,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
 
 /* ************************************************************************* */
 template<class FACTOR>
-vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
+FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
 
   // Number of factors and variables
   const size_t m = structure.nFactors();
@@ -67,8 +67,8 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& struc
   static const Index none = numeric_limits<Index>::max();
 
   // Allocate result parent vector and vector of last factor columns
-  vector<Index> parents(n, none);
-  vector<Index> prevCol(m, none);
+  FastVector<Index> parents(n, none);
+  FastVector<Index> prevCol(m, none);
 
   // for column j \in 1 to n do
   for (Index j = 0; j < n; j++) {
@@ -100,7 +100,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   tic(1, "ET ComputeParents");
   // Compute the tree structure
-  vector<Index> parents(ComputeParents(structure));
+  FastVector<Index> parents(ComputeParents(structure));
   toc(1, "ET ComputeParents");
 
   // Number of variables
@@ -110,7 +110,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   // Create tree structure
   tic(2, "assemble tree");
-  vector<shared_ptr> trees(n);
+  FastVector<shared_ptr> trees(n);
   for (Index k = 1; k <= n; k++) {
     Index j = n - k;  // Start at the last variable and loop down to 0
     trees[j].reset(new EliminationTree(j));  // Create a new node on this variable
diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h
index 4d0ccce56..4dab46dd3 100644
--- a/gtsam/inference/EliminationTree.h
+++ b/gtsam/inference/EliminationTree.h
@@ -54,7 +54,7 @@ private:
 
   typedef FastList<sharedFactor> Factors;
   typedef FastList<shared_ptr> SubTrees;
-  typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
+  typedef FastVector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
 
   Index key_; ///< index associated with root
   Factors factors_; ///< factors associated with root
@@ -74,7 +74,7 @@ private:
    * Static internal function to build a vector of parent pointers using the
    * algorithm of Gilbert et al., 2001, BIT.
    */
-  static std::vector<Index> ComputeParents(const VariableIndex& structure);
+  static FastVector<Index> ComputeParents(const VariableIndex& structure);
 
   /** add a factor, for Create use only */
   void add(const sharedFactor& factor) { factors_.push_back(factor); }
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 5a0b1e333..f51276ddc 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -22,10 +22,10 @@
 #pragma once
 
 #include <set>
-#include <vector>
 #include <boost/serialization/nvp.hpp>
 #include <boost/foreach.hpp>
-#include <gtsam/base/FastMap.h>
+
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -68,15 +68,15 @@ public:
   typedef boost::shared_ptr<Factor> shared_ptr;
 
   /// Iterator over keys
-  typedef typename std::vector<Key>::iterator iterator;
+  typedef typename FastVector<Key>::iterator iterator;
 
   /// Const iterator over keys
-  typedef typename std::vector<Key>::const_iterator const_iterator;
+  typedef typename FastVector<Key>::const_iterator const_iterator;
 
 protected:
 
   /// The keys involved in this factor
-  std::vector<Key> keys_;
+  FastVector<Key> keys_;
 
   friend class JacobianFactor;
   friend class HessianFactor;
@@ -132,6 +132,11 @@ public:
 		assertInvariants();
 	}
 
+  /** Construct n-way factor */
+  Factor(const FastVector<Key>& keys) : keys_(keys) {
+    assertInvariants();
+  }
+
   /** Constructor from a collection of keys */
   template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
         keys_(beginKey, endKey) { assertInvariants(); }
@@ -165,8 +170,8 @@ public:
   /// find
   const_iterator find(Key key) const { return std::find(begin(), end(), key); }
 
-  ///TODO: comment
-  const std::vector<Key>& keys() const { return keys_; }
+  /// @return keys involved in this factor
+  const FastVector<Key>& keys() const { return keys_; }
 
   /** iterators */
   const_iterator begin() const { return keys_.begin(); }	///TODO: comment
@@ -194,7 +199,7 @@ public:
   /**
    * @return keys involved in this factor
    */
-  std::vector<Key>& keys() { return keys_; }
+  FastVector<Key>& keys() { return keys_; }
 
   /** mutable iterators */
   iterator begin() { return keys_.begin(); }	///TODO: comment
diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 337b571c6..6244c959a 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -104,8 +104,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, std::vector<KEY> >& variableSlots) {
-		typedef const pair<const KEY, std::vector<KEY> > KeySlotPair;
+			const FastMap<KEY, FastVector<KEY> >& variableSlots) {
+		typedef const pair<const KEY, FastVector<KEY> > KeySlotPair;
 		return typename DERIVED::shared_ptr(new DERIVED(
 		    boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)),
 		    boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1))));
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 458588b7b..fcbd02a29 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -232,7 +232,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
   /** Create a combined joint factor (new style for EliminationTree). */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, std::vector<KEY> >& variableSlots);
+			const FastMap<KEY, FastVector<KEY> >& variableSlots);
 
 	/**
    * static function that combines two factor graphs
diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h
index c1cbbdaac..e8cb3f9c1 100644
--- a/gtsam/inference/IndexConditional.h
+++ b/gtsam/inference/IndexConditional.h
@@ -74,12 +74,23 @@ namespace gtsam {
 			assertInvariants();
 		}
 
+    /** Constructor from a frontal variable and a vector of parents (FastVector version) */
+    IndexConditional(Index j, const FastVector<Index>& parents) : Base(j, parents) {
+      assertInvariants();
+    }
+
     /** Constructor from keys and nr of frontal variables */
 		IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
 			Base(keys, nrFrontals) {
 			assertInvariants();
 		}
 
+    /** Constructor from keys and nr of frontal variables (FastVector version) */
+    IndexConditional(const FastVector<Index>& keys, size_t nrFrontals) :
+      Base(keys, nrFrontals) {
+      assertInvariants();
+    }
+
 		/// @}
 		/// @name Standard Interface
 		/// @{
diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h
index 9b2d39727..32488a9d5 100644
--- a/gtsam/inference/IndexFactor.h
+++ b/gtsam/inference/IndexFactor.h
@@ -114,6 +114,12 @@ namespace gtsam {
 			assertInvariants();
 		}
 
+    /** Construct n-way factor (FastVector version) */
+    IndexFactor(const FastVector<Index>& js) :
+      Base(js) {
+      assertInvariants();
+    }
+
 		/** Constructor from a collection of keys */
 		template<class KeyIterator> IndexFactor(KeyIterator beginKey,
 				KeyIterator endKey) :
diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h
index ad8b389ab..739b51048 100644
--- a/gtsam/inference/JunctionTree-inl.h
+++ b/gtsam/inference/JunctionTree-inl.h
@@ -83,7 +83,7 @@ namespace gtsam {
 
     // Two stages - first build an array of the lowest-ordered variable in each
     // factor and find the last variable to be eliminated.
-    vector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
+    FastVector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
     Index maxVar = 0;
     for(size_t i=0; i<fg.size(); ++i)
       if(fg[i]) {
@@ -96,7 +96,7 @@ namespace gtsam {
 
     // Now add each factor to the list corresponding to its lowest-ordered
     // variable.
-    vector<FastList<size_t> > targets(maxVar+1);
+    FastVector<FastList<size_t> > targets(maxVar+1);
     for(size_t i=0; i<lowestOrdered.size(); ++i)
       if(lowestOrdered[i] != numeric_limits<Index>::max())
         targets[lowestOrdered[i]].push_back(i);
@@ -108,7 +108,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class FG, class BTCLIQUE>
   typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
-      const std::vector<FastList<size_t> >& targets,
+      const FastVector<FastList<size_t> >& targets,
       const SymbolicBayesTree::sharedClique& bayesClique) {
 
     if(bayesClique) {
diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h
index 33a9aa5e4..a068be991 100644
--- a/gtsam/inference/JunctionTree.h
+++ b/gtsam/inference/JunctionTree.h
@@ -20,7 +20,7 @@
 #pragma once
 
 #include <set>
-#include <vector>
+#include <gtsam/base/FastVector.h>
 #include <gtsam/base/FastList.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/ClusterTree.h>
@@ -78,7 +78,7 @@ namespace gtsam {
 				const SymbolicBayesTree::sharedClique& clique);
 
 		/// distribute the factors along the cluster tree
-    sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
+    sharedClique distributeFactors(const FG& fg, const FastVector<FastList<size_t> >& targets,
         const SymbolicBayesTree::sharedClique& clique);
 
 		/// recursive elimination function
diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h
index 5cbd8e187..bde6dd459 100644
--- a/gtsam/inference/Permutation.h
+++ b/gtsam/inference/Permutation.h
@@ -17,12 +17,12 @@
 
 #pragma once
 
-#include <vector>
 #include <string>
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -46,12 +46,12 @@ class Inference;
  */
 class Permutation {
 protected:
-  std::vector<Index> rangeIndices_;
+  FastVector<Index> rangeIndices_;
 
 public:
   typedef boost::shared_ptr<Permutation> shared_ptr;
-  typedef std::vector<Index>::const_iterator const_iterator;
-  typedef std::vector<Index>::iterator iterator;
+  typedef FastVector<Index>::const_iterator const_iterator;
+  typedef FastVector<Index>::iterator iterator;
 
 	/// @name Standard Constructors
 	/// @{
diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp
index 2d5190f45..233c1e21c 100644
--- a/gtsam/inference/SymbolicFactorGraph.cpp
+++ b/gtsam/inference/SymbolicFactorGraph.cpp
@@ -66,7 +66,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-					std::vector<Index> >& variableSlots) {
+			FastVector<Index> >& variableSlots) {
 		IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
 				variableSlots));
 //		combined->assertInvariants();
@@ -84,9 +84,11 @@ namespace gtsam {
 
 		if (keys.size() < 1) throw invalid_argument(
 				"IndexFactor::CombineAndEliminate called on factors with no variables.");
+		if(nrFrontals > keys.size()) throw invalid_argument(
+		    "Requesting to eliminate more variables than are present in the factors");
 
 		pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
-		std::vector<Index> newKeys(keys.begin(),keys.end());
+		FastVector<Index> newKeys(keys.begin(),keys.end());
     result.first.reset(new IndexConditional(newKeys, nrFrontals));
 		result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
 
diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h
index 4da2baf13..e39e4a70b 100644
--- a/gtsam/inference/SymbolicFactorGraph.h
+++ b/gtsam/inference/SymbolicFactorGraph.h
@@ -84,7 +84,7 @@ namespace gtsam {
 	/** Create a combined joint factor (new style for EliminationTree). */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-					std::vector<Index> >& variableSlots);
+			FastVector<Index> >& variableSlots);
 
 	/**
 	 * CombineAndEliminate provides symbolic elimination.
diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp
index d30ea79c4..8ec67b26c 100644
--- a/gtsam/inference/inference.cpp
+++ b/gtsam/inference/inference.cpp
@@ -29,12 +29,12 @@ using namespace std;
 
 namespace gtsam {
 
-Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
+Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember) {
   size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
   int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
-  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
-  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+  vector<int> A(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  vector<int> p(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
 
   static const bool debug = false;
 
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index d853ff91b..728d2c4bd 100644
--- a/gtsam/inference/inference.h
+++ b/gtsam/inference/inference.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/Permutation.h>
 
@@ -48,7 +49,7 @@ namespace gtsam {
 	  /**
 	   * Compute a CCOLAMD permutation using the constraint groups in cmember.
 	   */
-    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
+    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember);
 
 	};
 
@@ -56,7 +57,7 @@ namespace gtsam {
 	template<typename CONSTRAINED>
 	Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
 
-	  std::vector<int> cmember(variableIndex.size(), 0);
+	  FastVector<int> cmember(variableIndex.size(), 0);
 
 	  // If at least some variables are not constrained to be last, constrain the
 	  // ones that should be constrained.
@@ -72,7 +73,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
-	  std::vector<int> cmember(variableIndex.size(), 0);
+	  FastVector<int> cmember(variableIndex.size(), 0);
 	  return PermutationCOLAMD_(variableIndex, cmember);
 	}
 
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 7374bbad8..9695afc56 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -56,15 +56,15 @@ TEST( JunctionTree, constructor )
 
 	SymbolicJunctionTree actual(fg);
 
-	vector<Index> frontal1; frontal1 += x3, x4;
-	vector<Index> frontal2; frontal2 += x2, x1;
-	vector<Index> sep1;
-	vector<Index> sep2; sep2 += x3;
-	CHECK(assert_equal(frontal1, actual.root()->frontal));
-	CHECK(assert_equal(sep1,     actual.root()->separator));
+	FastVector<Index> frontal1; frontal1 += x3, x4;
+	FastVector<Index> frontal2; frontal2 += x2, x1;
+	FastVector<Index> sep1;
+	FastVector<Index> sep2; sep2 += x3;
+	CHECK(assert_container_equality(frontal1, actual.root()->frontal));
+	CHECK(assert_container_equality(sep1,     actual.root()->separator));
 	LONGS_EQUAL(1,               actual.root()->size());
-	CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
-	CHECK(assert_equal(sep2,     actual.root()->children().front()->separator));
+	CHECK(assert_container_equality(frontal2, actual.root()->children().front()->frontal));
+	CHECK(assert_container_equality(sep2,     actual.root()->children().front()->separator));
 	LONGS_EQUAL(2,               actual.root()->children().front()->size());
 	CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
   CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index f2a77c894..af83a6f44 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -268,7 +268,7 @@ template<typename ITERATOR, class MATRIX>
 GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
 		size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
 		const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) :
-	IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
+	IndexConditional(FastVector<Index>(firstKey, lastKey), nrFrontals), rsd_(
 			matrix_), sigmas_(sigmas), permutation_(permutation) {
 	rsd_.assignNoalias(matrices);
 }
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index 2d889ee64..771a3de8f 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -74,6 +74,9 @@ namespace gtsam {
     /** Construct n-way factor */
     GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
 
+    /** Construct n-way factor */
+    GaussianFactor(const FastVector<Index>& js) : IndexFactor(js) {}
+
   public:
 
     typedef GaussianConditional ConditionalType;
@@ -110,8 +113,8 @@ namespace gtsam {
 
   /** make keys from list, vector, or map of matrices */
 	template<typename ITERATOR>
-	static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
-		std::vector<Index> keys;
+	static FastVector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
+	  FastVector<Index> keys;
 		keys.reserve(n);
 		for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
 		return keys;
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 71d111149..7efeea10d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -168,11 +168,11 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	// Helper functions for Combine
-	static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
+	static boost::tuple<FastVector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
 #ifndef NDEBUG
-		vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
+	  FastVector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
 #else
-		vector<size_t> varDims(variableSlots.size());
+	  FastVector<size_t> varDims(variableSlots.size());
 #endif
 		size_t m = 0;
 		size_t n = 0;
@@ -221,7 +221,7 @@ break;
 
 		if (debug) cout << "Determine dimensions" << endl;
 		tic(1, "countDims");
-		vector<size_t> varDims;
+		FastVector<size_t> varDims;
 		size_t m, n;
 		boost::tie(varDims, m, n) = countDims(factors, variableSlots);
 		if (debug) {
@@ -233,7 +233,7 @@ break;
 
 		if (debug) cout << "Sort rows" << endl;
 		tic(2, "sort rows");
-		vector<JacobianFactor::_RowSource> rowSources;
+		FastVector<JacobianFactor::_RowSource> rowSources;
 		rowSources.reserve(m);
 		bool anyConstrained = false;
 		for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
@@ -367,7 +367,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		vector<size_t> dimensions(scatter.size() + 1);
+		FastVector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
@@ -569,7 +569,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		vector<size_t> dimensions(scatter.size() + 1);
+		FastVector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp
index e2e1c2951..0db558a10 100644
--- a/gtsam/linear/GaussianJunctionTree.cpp
+++ b/gtsam/linear/GaussianJunctionTree.cpp
@@ -21,8 +21,6 @@
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 
-#include <vector>
-
 #include <boost/foreach.hpp>
 
 namespace gtsam {
@@ -68,7 +66,7 @@ namespace gtsam {
 
 		// Allocate solution vector and copy RHS
     tic(2, "allocate VectorValues");
-    vector<size_t> dims(rootClique->conditional()->back()+1, 0);
+    FastVector<size_t> dims(rootClique->conditional()->back()+1, 0);
 		countDims(rootClique, dims);
 		VectorValues result(dims);
 		btreeRHS(rootClique, result);
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 8402e4f91..eedecc4ed 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
-		const vector<size_t>& dimensions, const Scatter& scatter) :
+		const FastVector<size_t>& dimensions, const Scatter& scatter) :
 		info_(matrix_) {
 
 	const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
@@ -505,7 +505,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
   tic(2, "remaining factor");
   info_.blockStart() = nrFrontals;
   // Assign the keys
-  vector<Index> remainingKeys(keys_.size() - nrFrontals);
+  FastVector<Index> remainingKeys(keys_.size() - nrFrontals);
   remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
   keys_.swap(remainingKeys);
   toc(2, "remaining factor");
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 2a16c0e38..6225fcde9 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -204,7 +204,7 @@ namespace gtsam {
 
     /** Special constructor used in EliminateCholesky which combines the given factors */
     HessianFactor(const FactorGraph<GaussianFactor>& factors,
-				const std::vector<size_t>& dimensions, const Scatter& scatter);
+				const FastVector<size_t>& dimensions, const Scatter& scatter);
 
     /** Destructor */
 		virtual ~HessianFactor() {}
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 0218ecc8d..064143512 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -113,7 +113,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
+  JacobianFactor::JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
   		const Vector &b, const SharedDiagonal& model) :
   	GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
 		model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
@@ -260,7 +260,7 @@ namespace gtsam {
       sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
 
     // Build a vector of variable dimensions in the new order
-    vector<size_t> dimensions(size() + 1);
+    FastVector<size_t> dimensions(size() + 1);
     size_t j = 0;
     BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
       dimensions[j++] = Ab_(sourceSlot.second).cols();
@@ -269,7 +269,7 @@ namespace gtsam {
     dimensions.back() = 1;
 
     // Copy the variables and matrix into the new order
-    vector<Index> oldKeys(size());
+    FastVector<Index> oldKeys(size());
     keys_.swap(oldKeys);
     AbMatrix oldMatrix;
     BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
@@ -506,7 +506,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
+  void JacobianFactor::collectInfo(size_t index, FastVector<_RowSource>& rowSources) const {
 		assertInvariants();
 		for (size_t row = 0; row < rows(); ++row) {
 			Index firstNonzeroVar;
@@ -522,7 +522,7 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  void JacobianFactor::allocate(const VariableSlots& variableSlots, vector<
+  void JacobianFactor::allocate(const VariableSlots& variableSlots, FastVector<
 			size_t>& varDims, size_t m) {
 		keys_.resize(variableSlots.size());
 		std::transform(variableSlots.begin(), variableSlots.end(), begin(),
@@ -551,7 +551,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void JacobianFactor::copyFNZ(size_t m, size_t n,
-			vector<_RowSource>& rowSources) {
+      FastVector<_RowSource>& rowSources) {
 		Index i = 0;
 		for (size_t row = 0; row < m; ++row) {
 			while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 079cb9292..90270ce95 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -85,7 +85,7 @@ namespace gtsam {
   protected:
 
     SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
-    std::vector<size_t> firstNonzeroBlocks_;
+    FastVector<size_t> firstNonzeroBlocks_;
     AbMatrix matrix_; // the full matrix corresponding to the factor
     BlockAb Ab_;      // the block view of the full matrix
     typedef GaussianFactor Base; // typedef to base
@@ -123,7 +123,7 @@ namespace gtsam {
         const Vector& b, const SharedDiagonal& model);
 
     /** Construct an n-ary factor */
-    JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
+    JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
         const Vector &b, const SharedDiagonal& model);
 
     JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms,
@@ -268,18 +268,18 @@ namespace gtsam {
     // Many imperative, perhaps all need to be combined in constructor
 
     /** Collect information on Jacobian rows */
-    void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const;
+    void collectInfo(size_t index, FastVector<_RowSource>& rowSources) const;
 
     /** allocate space */
     void allocate(const VariableSlots& variableSlots,
-				std::vector<size_t>& varDims, size_t m);
+        FastVector<size_t>& varDims, size_t m);
 
     /** copy a slot from source */
     void copyRow(const JacobianFactor& source,
     		Index sourceRow, size_t sourceSlot, size_t row, Index slot);
 
     /** copy firstNonzeroBlocks structure */
-    void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources);
+    void copyFNZ(size_t m, size_t n, FastVector<_RowSource>& rowSources);
 
     /** set noiseModel correctly */
   	void setModel(bool anyConstrained, const Vector& sigmas);
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index fd93d39fc..37584af2e 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -163,7 +163,7 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
   return Unit::Create(maxrank);
 }
 
-void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
+void Gaussian::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
   BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
   whitenInPlace(b);
 }
@@ -513,7 +513,7 @@ Vector Base::sqrtWeight(const Vector &error) const {
  * according to their weight implementation */
 
 /** Reweight n block matrices with one error vector */
-void Base::reweight(vector<Matrix> &A, Vector &error) const {
+void Base::reweight(FastVector<Matrix> &A, Vector &error) const {
   if ( reweight_ == Block ) {
     const double w = sqrtWeight(error.norm());
     BOOST_FOREACH(Matrix& Aj, A) {
@@ -662,7 +662,7 @@ bool Robust::equals(const Base& expected, double tol) const {
   return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
 }
 
-void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
+void Robust::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
   noise_->WhitenSystem(A,b);
   robust_->reweight(A,b);
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index ee5d9e00c..640eadfba 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -20,6 +20,7 @@
 
 #include <boost/serialization/nvp.hpp>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -76,7 +77,7 @@ namespace gtsam {
 
       virtual double distance(const Vector& v) const = 0;
 
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
@@ -185,7 +186,7 @@ namespace gtsam {
 			/**
 			 * Whiten a system, in place as well
 			 */
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const ;
 			virtual void WhitenSystem(Matrix& A, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
@@ -619,7 +620,7 @@ namespace gtsam {
       Vector sqrtWeight(const Vector &error) const;
 
       /** reweight block matrices and a vector according to their weight implementation */
-      void reweight(std::vector<Matrix> &A, Vector &error) const;
+      void reweight(FastVector<Matrix> &A, Vector &error) const;
 		  void reweight(Matrix &A, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
@@ -714,7 +715,7 @@ namespace gtsam {
 
       // TODO: these are really robust iterated re-weighting support functions
 
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index d0d37d886..b90b2c4ea 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -57,7 +57,7 @@ namespace gtsam {
     if(recalculate) {
 
       // Temporary copy of the original values, to check how much they change
-      vector<Vector> originalValues((*clique)->nrFrontals());
+      FastVector<Vector> originalValues((*clique)->nrFrontals());
       GaussianConditional::const_iterator it;
       for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
         originalValues[it - (*clique)->beginFrontals()] = delta[*it];
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index eae39bf52..4858b1900 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -126,7 +126,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
   theta.insert(newTheta);
   if(debug) newTheta.print("The new variables are: ");
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
-  std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
+  FastVector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
   const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
   const size_t originalDim = delta->dim();
@@ -287,7 +287,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
   if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
   toc(2,"variable index");
   tic(3,"ccolamd");
-  vector<int> cmember(affectedKeysSelector.size(), 0);
+  FastVector<int> cmember(affectedKeysSelector.size(), 0);
   if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
     assert(reorderingMode.constrainedKeys);
     if(keys.size() > reorderingMode.constrainedKeys->size()) {
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 5c87d1dda..6dc47131f 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -229,7 +229,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     tic(1,"reorder");
     tic(1,"CCOLAMD");
     // Do a batch step - reorder and relinearize all variables
-    vector<int> cmember(theta_.size(), 0);
+    FastVector<int> cmember(theta_.size(), 0);
     FastSet<Index> constrainedKeysSet;
     if(constrainKeys)
       constrainedKeysSet = *constrainKeys;
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 166ad6980..512d7af91 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,7 +24,6 @@
 #include <limits>
 
 #include <boost/serialization/base_object.hpp>
-#include <boost/tuple/tuple.hpp>
 
 #include <gtsam/inference/Factor-inl.h>
 #include <gtsam/inference/IndexFactor.h>
@@ -36,19 +35,6 @@
 
 namespace gtsam {
 
-using boost::make_tuple;
-
-// Helper function to fill a vector from a tuple function of any length
-template<typename CONS>
-inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
-  vector[position] = tuple.get_head();
-  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
-}
-template<>
-inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
-  // Do nothing
-}
-
 /* ************************************************************************* */
 /**
  * Nonlinear factor base class
@@ -139,7 +125,7 @@ public:
    * variable indices.
    */
   virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
-    std::vector<Index> indices(this->size());
+    FastVector<Index> indices(this->size());
     for(size_t j=0; j<this->size(); ++j)
       indices[j] = ordering[this->keys()[j]];
     return IndexFactor::shared_ptr(new IndexFactor(indices));
@@ -228,7 +214,7 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
@@ -264,12 +250,12 @@ public:
     // Create the set of terms - Jacobians for each index
     Vector b;
     // Call evaluate error to get Jacobians and b vector
-    std::vector<Matrix> A(this->size());
+    FastVector<Matrix> A(this->size());
     b = -unwhitenedError(x, A);
 
     this->noiseModel_->WhitenSystem(A,b);
 
-    std::vector<std::pair<Index, Matrix> > terms(this->size());
+    FastVector<std::pair<Index, Matrix> > terms(this->size());
     // Fill in terms
     for(size_t j=0; j<this->size(); ++j) {
       terms[j].first = ordering[this->keys()[j]];
@@ -339,7 +325,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x.at<X>(keys_[0]);
       if(H) {
@@ -422,7 +408,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x.at<X1>(keys_[0]);
       const X2& x2 = x.at<X2>(keys_[1]);
@@ -512,7 +498,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@@ -607,7 +593,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -707,7 +693,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -813,7 +799,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index 19b0c676d..24d8e0046 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -59,29 +59,29 @@ TEST( GaussianJunctionTree, constructor2 )
 	// create an ordering
 	GaussianJunctionTree actual(fg);
 
-	vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
-	vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
-	vector<Index> frontal3; frontal3 += ordering["x1"];
-	vector<Index> frontal4; frontal4 += ordering["x7"];
-	vector<Index> sep1;
-	vector<Index> sep2; sep2 += ordering["x4"];
-	vector<Index> sep3; sep3 += ordering["x2"];
-	vector<Index> sep4; sep4 += ordering["x6"];
-	EXPECT(assert_equal(frontal1, actual.root()->frontal));
-	EXPECT(assert_equal(sep1,     actual.root()->separator));
+	FastVector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
+	FastVector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
+	FastVector<Index> frontal3; frontal3 += ordering["x1"];
+	FastVector<Index> frontal4; frontal4 += ordering["x7"];
+	FastVector<Index> sep1;
+	FastVector<Index> sep2; sep2 += ordering["x4"];
+	FastVector<Index> sep3; sep3 += ordering["x2"];
+	FastVector<Index> sep4; sep4 += ordering["x6"];
+	EXPECT(assert_container_equality(frontal1, actual.root()->frontal));
+	EXPECT(assert_container_equality(sep1,     actual.root()->separator));
 	LONGS_EQUAL(5,               actual.root()->size());
 	list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
   list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
   GaussianJunctionTree::sharedClique child0 = *child0it;
   GaussianJunctionTree::sharedClique child1 = *child1it;
-	EXPECT(assert_equal(frontal2, child0->frontal));
-	EXPECT(assert_equal(sep2,     child0->separator));
+	EXPECT(assert_container_equality(frontal2, child0->frontal));
+	EXPECT(assert_container_equality(sep2,     child0->separator));
 	LONGS_EQUAL(4,               child0->size());
-	EXPECT(assert_equal(frontal3, child0->children().front()->frontal));
-	EXPECT(assert_equal(sep3,     child0->children().front()->separator));
+	EXPECT(assert_container_equality(frontal3, child0->children().front()->frontal));
+	EXPECT(assert_container_equality(sep3,     child0->children().front()->separator));
 	LONGS_EQUAL(2,               child0->children().front()->size());
-	EXPECT(assert_equal(frontal4, child1->frontal));
-	EXPECT(assert_equal(sep4,     child1->separator));
+	EXPECT(assert_container_equality(frontal4, child1->frontal));
+	EXPECT(assert_container_equality(sep4,     child1->separator));
 	LONGS_EQUAL(2,               child1->size());
 }