diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index c3352dfd5..8c23ae9e5 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -18,29 +18,22 @@
 
 #pragma once
 
-#include <gtsam/base/FastDefaultAllocator.h>
-
-#include <boost/mpl/has_xxx.hpp>
-#include <boost/utility/enable_if.hpp>
-#include <boost/serialization/serialization.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
+#include <gtsam/base/FastDefaultAllocator.h>
+#include <gtsam/base/Testable.h>
 
-#include <cmath>
-#include <iosfwd>
-#include <iostream>
+#include <functional>
 #include <set>
-#include <string>
 
-BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
+namespace boost {
+namespace serialization {
+class access;
+} /* namespace serialization */
+} /* namespace boost */
 
 namespace gtsam {
 
-// This is used internally to allow this container to be Testable even when it
-// contains non-testable elements.
-template<typename VALUE, class ENABLE = void>
-struct FastSetTestableHelper;
-
 /**
  * FastSet is a thin wrapper around std::set that uses the boost
  * fast_pool_allocator instead of the default STL allocator.  This is just a
@@ -48,12 +41,16 @@ struct FastSetTestableHelper;
  * we've seen that the fast_pool_allocator can lead to speedups of several %.
  * @addtogroup base
  */
-template<typename VALUE, class ENABLE = void>
-class FastSet: public std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> {
+template<typename VALUE>
+class FastSet: public std::set<VALUE, std::less<VALUE>,
+    typename internal::FastDefaultAllocator<VALUE>::type> {
+
+  BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
 
 public:
 
-  typedef std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> Base;
+  typedef std::set<VALUE, std::less<VALUE>,
+  typename internal::FastDefaultAllocator<VALUE>::type> Base;
 
   /** Default constructor */
   FastSet() {
@@ -62,23 +59,23 @@ public:
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
   explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) :
-      Base(first, last) {
+  Base(first, last) {
   }
 
   /** Constructor from a iterable container, passes through to base class */
   template<typename INPUTCONTAINER>
   explicit FastSet(const INPUTCONTAINER& container) :
-      Base(container.begin(), container.end()) {
+  Base(container.begin(), container.end()) {
   }
 
   /** Copy constructor from another FastSet */
   FastSet(const FastSet<VALUE>& x) :
-      Base(x) {
+  Base(x) {
   }
 
   /** Copy constructor from the base set class */
   FastSet(const Base& x) :
-      Base(x) {
+  Base(x) {
   }
 
 #ifdef GTSAM_ALLOCATOR_BOOSTPOOL
@@ -88,7 +85,7 @@ public:
     // STL vector where if the size is zero, the pool allocator will allocate
     // huge amounts of memory.
     if(x.size() > 0)
-      Base::insert(x.begin(), x.end());
+    Base::insert(x.begin(), x.end());
   }
 #endif
 
@@ -98,17 +95,31 @@ public:
   }
 
   /** Handy 'exists' function */
-  bool exists(const VALUE& e) const { return this->find(e) != this->end(); }
+  bool exists(const VALUE& e) const {
+    return this->find(e) != this->end();
+  }
 
-  /** Print to implement Testable */
-  void print(const std::string& str = "") const { FastSetTestableHelper<VALUE>::print(*this, str); }
+  /** Print to implement Testable: pretty basic */
+  void print(const std::string& str = "") const {
+    for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it)
+    traits<VALUE>::Print(*it, str);
+  }
 
   /** Check for equality within tolerance to implement Testable */
-  bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const { return FastSetTestableHelper<VALUE>::equals(*this, other, tol); }
+  bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const {
+    typename Base::const_iterator it1 = this->begin(), it2 = other.begin();
+    while (it1 != this->end()) {
+      if (it2 == other.end() || !traits<VALUE>::Equals(*it2, *it2, tol))
+      return false;
+      ++it1;
+      ++it2;
+    }
+    return true;
+  }
 
   /** insert another set: handy for MATLAB access */
   void merge(const FastSet& other) {
-    Base::insert(other.begin(),other.end());
+    Base::insert(other.begin(), other.end());
   }
 
 private:
@@ -120,59 +131,4 @@ private:
   }
 };
 
-// This is the default Testable interface for *non*Testable elements, which
-// uses stream operators.
-template<typename VALUE, class ENABLE>
-struct FastSetTestableHelper {
-
-  typedef FastSet<VALUE> Set;
-
-  static void print(const Set& set, const std::string& str) {
-    std::cout << str << "\n";
-    for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
-      std::cout << "  " << *it << "\n";
-    std::cout.flush();
-  }
-
-  static bool equals(const Set& set1, const Set& set2, double tol) {
-    typename Set::const_iterator it1 = set1.begin();
-    typename Set::const_iterator it2 = set2.begin();
-    while (it1 != set1.end()) {
-      if (it2 == set2.end() ||
-          fabs((double)(*it1) - (double)(*it2)) > tol)
-        return false;
-      ++it1;
-      ++it2;
-    }
-    return true;
-  }
-};
-
-// This is the Testable interface for Testable elements
-template<typename VALUE>
-struct FastSetTestableHelper<VALUE, typename boost::enable_if<has_print<VALUE> >::type> {
-
-  typedef FastSet<VALUE> Set;
-
-  static void print(const Set& set, const std::string& str) {
-    std::cout << str << "\n";
-    for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
-      it->print("  ");
-    std::cout.flush();
-  }
-
-  static bool equals(const Set& set1, const Set& set2, double tol) {
-    typename Set::const_iterator it1 = set1.begin();
-    typename Set::const_iterator it2 = set2.begin();
-    while (it1 != set1.end()) {
-      if (it2 == set2.end() ||
-          !it1->equals(*it2, tol))
-        return false;
-      ++it1;
-      ++it2;
-    }
-    return true;
-  }
-};
-
 }
diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp
index 464624bd4..19870fdeb 100644
--- a/gtsam/base/tests/testFastContainers.cpp
+++ b/gtsam/base/tests/testFastContainers.cpp
@@ -7,13 +7,14 @@
  * @author Alex Cunningham
  */
 
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/set.hpp>
 
-#include <gtsam/base/FastSet.h>
-#include <gtsam/base/FastVector.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace boost::assign;
 using namespace gtsam;
@@ -21,7 +22,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( testFastContainers, KeySet ) {
 
-  FastVector<size_t> init_vector;
+  FastVector<Key> init_vector;
   init_vector += 2, 3, 4, 5;
 
   FastSet<size_t> actSet(init_vector);
diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp
index fceb2f4b4..1db28bcc8 100644
--- a/gtsam/base/tests/testSerializationBase.cpp
+++ b/gtsam/base/tests/testSerializationBase.cpp
@@ -16,6 +16,8 @@
  * @date Feb 7, 2012
  */
 
+#include <gtsam/inference/Key.h>
+
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/FastList.h>
@@ -60,10 +62,10 @@ TEST (Serialization, FastMap) {
 
 /* ************************************************************************* */
 TEST (Serialization, FastSet) {
-  FastSet<double> set;
-  set.insert(1.0);
-  set.insert(2.0);
-  set.insert(3.0);
+  KeySet set;
+  set.insert(1);
+  set.insert(2);
+  set.insert(3);
 
   EXPECT(equality(set));
   EXPECT(equalityXML(set));
diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
deleted file mode 100644
index 03e7fd120..000000000
--- a/gtsam/base/types.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     types.h
- * @brief    Typedefs for easier changing of types
- * @author   Richard Roberts
- * @date     Aug 21, 2010
- * @addtogroup base
- */
-
-#include <gtsam/base/types.h>
-#include <gtsam/inference/Symbol.h>
-#include <boost/lexical_cast.hpp>
-
-namespace gtsam {
-
-  /* ************************************************************************* */
-  std::string _defaultKeyFormatter(Key key) {
-    const Symbol asSymbol(key);
-    if(asSymbol.chr() > 0)
-      return (std::string)asSymbol;
-    else
-      return boost::lexical_cast<std::string>(key);
-  }
-
-}
\ No newline at end of file
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index 4010eb70e..69e4e4192 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -21,7 +21,6 @@
 
 #include <gtsam/dllexport.h>
 #include <boost/concept/assert.hpp>
-#include <boost/function.hpp>
 #include <boost/range/concepts.hpp>
 #include <cstddef>
 
@@ -54,18 +53,6 @@ namespace gtsam {
   /// Integer nonlinear key type
   typedef size_t Key;
 
-  /// Typedef for a function to format a key, i.e. to convert it to a string
-  typedef boost::function<std::string(Key)> KeyFormatter;
-
-  // Helper function for DefaultKeyFormatter
-  GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
-
-  /// The default KeyFormatter, which is used if no KeyFormatter is passed to
-  /// a nonlinear 'print' function.  Automatically detects plain integer keys
-  /// and Symbol keys.
-  static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
-
-
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 77be1d277..c2128c776 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -39,8 +39,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  FastSet<Key> DiscreteFactorGraph::keys() const {
-    FastSet<Key> keys;
+  KeySet DiscreteFactorGraph::keys() const {
+    KeySet keys;
     BOOST_FOREACH(const sharedFactor& factor, *this)
     if (factor) keys.insert(factor->begin(), factor->end());
     return keys;
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index c5b11adf1..cdfd7d522 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -120,7 +120,7 @@ public:
   }
 
   /** Return the set of variables involved in the factors (set union) */
-  FastSet<Key> keys() const;
+  KeySet keys() const;
 
   /** return product of all factors as a single factor */
   DecisionTreeFactor product() const;
diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h
index 978781d63..1078b4c61 100644
--- a/gtsam/discrete/Potentials.h
+++ b/gtsam/discrete/Potentials.h
@@ -19,6 +19,7 @@
 
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/inference/Key.h>
 
 #include <boost/shared_ptr.hpp>
 #include <set>
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 281648409..3a3e1317c 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -344,7 +344,7 @@ namespace gtsam {
       gttic(Full_root_factoring);
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
         FastVector<Key> C1_minus_B; {
-          FastSet<Key> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
+          KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
           BOOST_FOREACH(const Key j, *B->conditional()) {
             C1_minus_B_set.erase(j); }
           C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
@@ -356,7 +356,7 @@ namespace gtsam {
       }
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
         FastVector<Key> C2_minus_B; {
-          FastSet<Key> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
+          KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
           BOOST_FOREACH(const Key j, *B->conditional()) {
             C2_minus_B_set.erase(j); }
           C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index ff50c9781..4d68acb5b 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -19,13 +19,13 @@
 
 #pragma once
 
-#include <string>
-
-#include <gtsam/base/types.h>
+#include <gtsam/inference/Key.h>
 #include <gtsam/base/FastList.h>
 #include <gtsam/base/ConcurrentMap.h>
 #include <gtsam/base/FastVector.h>
 
+#include <string>
+
 namespace gtsam {
 
   // Forward declarations
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index 274886c21..256ff983d 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inst.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inst.h
@@ -44,8 +44,8 @@ namespace gtsam {
   FastVector<Key>
     BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
   {
-    FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
-    FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
+    KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
+    KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
     FastVector<Key> S_setminus_B;
     std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
       indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
@@ -58,8 +58,8 @@ namespace gtsam {
     const derived_ptr& B, const FactorGraphType& p_Cp_B) const
   {
     gttic(shortcut_indices);
-    FastSet<Key> allKeys = p_Cp_B.keys();
-    FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
+    KeySet allKeys = p_Cp_B.keys();
+    KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
     FastVector<Key> S_setminus_B = separator_setminus_B(B);
     FastVector<Key> keep;
     // keep = S\B intersect allKeys (S_setminus_B is already sorted)
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index 8c19d4ff4..c629d336a 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/gtsam/inference/FactorGraph-inst.h
@@ -28,6 +28,7 @@
 
 #include <stdio.h>
 #include <sstream>
+#include <iostream> // for cout :-(
 
 namespace gtsam {
 
@@ -72,8 +73,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class FACTOR>
-  FastSet<Key> FactorGraph<FACTOR>::keys() const {
-    FastSet<Key> allKeys;
+  KeySet FactorGraph<FACTOR>::keys() const {
+    KeySet allKeys;
     BOOST_FOREACH(const sharedFactor& factor, factors_)
       if (factor)
         allKeys.insert(factor->begin(), factor->end());
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 4d5428e5c..e97860eaa 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -332,7 +332,7 @@ namespace gtsam {
     size_t nrFactors() const;
 
     /** Potentially very slow function to return all keys involved */
-    FastSet<Key> keys() const;
+    KeySet keys() const;
 
     /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
     inline bool exists(size_t idx) const { return idx < size() && at(idx); }
diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index 7cb3d9817..8be01ac5f 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/gtsam/inference/ISAM-inst.h
@@ -29,7 +29,7 @@ namespace gtsam {
     // Remove the contaminated part of the Bayes tree
     BayesNetType bn;
     if (!this->empty()) {
-      const FastSet<Key> newFactorKeys = newFactors.keys();
+      const KeySet newFactorKeys = newFactors.keys();
       this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
     }
 
@@ -44,7 +44,7 @@ namespace gtsam {
 
     // eliminate into a Bayes net
     const VariableIndex varIndex(factors);
-    const FastSet<Key> newFactorKeys = newFactors.keys();
+    const KeySet newFactorKeys = newFactors.keys();
     const Ordering constrainedOrdering =
       Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
     Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp
index 0b9be2f1c..a2a6906d1 100644
--- a/gtsam/inference/Key.cpp
+++ b/gtsam/inference/Key.cpp
@@ -17,57 +17,72 @@
  * @date Feb 20, 2012
  */
 
-#include <iostream>
-
-#include <boost/lexical_cast.hpp>
-#include <boost/foreach.hpp>
-
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/LabeledSymbol.h>
 
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+#include <iostream>
+
+using namespace std;
+
 namespace gtsam {
 
 /* ************************************************************************* */
-std::string _multirobotKeyFormatter(Key key) {
+string _defaultKeyFormatter(Key key) {
+  const Symbol asSymbol(key);
+  if (asSymbol.chr() > 0)
+    return (string) asSymbol;
+  else
+    return boost::lexical_cast<string>(key);
+}
+
+/* ************************************************************************* */
+void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) {
+  cout << s << keyFormatter(key);
+}
+
+/* ************************************************************************* */
+string _multirobotKeyFormatter(Key key) {
   const LabeledSymbol asLabeledSymbol(key);
   if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0)
-    return (std::string) asLabeledSymbol;
+    return (string) asLabeledSymbol;
 
   const Symbol asSymbol(key);
   if (asLabeledSymbol.chr() > 0)
-    return (std::string) asSymbol;
+    return (string) asSymbol;
   else
-    return boost::lexical_cast<std::string>(key);
+    return boost::lexical_cast<string>(key);
 }
 
 /* ************************************************************************* */
 template<class CONTAINER>
-static void print(const CONTAINER& keys, const std::string& s,
+static void Print(const CONTAINER& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  std::cout << s << " ";
+  cout << s << " ";
   if (keys.empty())
-    std::cout << "(none)" << std::endl;
+    cout << "(none)" << endl;
   else {
     BOOST_FOREACH(const Key& key, keys)
-      std::cout << keyFormatter(key) << " ";
-    std::cout << std::endl;
+      cout << keyFormatter(key) << " ";
+    cout << endl;
   }
 }
 
 /* ************************************************************************* */
-void printKeyList(const KeyList& keys, const std::string& s,
+void PrintKeyList(const KeyList& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
-void printKeyVector(const KeyVector& keys, const std::string& s,
+void PrintKeyVector(const KeyVector& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
-void printKeySet(const KeySet& keys, const std::string& s,
+void PrintKeySet(const KeySet& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
 
diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h
index e2be79dc7..d2b342575 100644
--- a/gtsam/inference/Key.h
+++ b/gtsam/inference/Key.h
@@ -17,45 +17,75 @@
  */
 #pragma once
 
-#include <boost/function.hpp>
-#include <string>
-
-#include <gtsam/global_includes.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/base/FastList.h>
-#include <gtsam/base/FastSet.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/types.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/function.hpp>
 
 namespace gtsam {
 
+/// Typedef for a function to format a key, i.e. to convert it to a string
+typedef boost::function<std::string(Key)> KeyFormatter;
 
-  // Helper function for Multi-robot Key Formatter
-  GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
+// Helper function for DefaultKeyFormatter
+GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
 
-  ///
-  /// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
-  /// integer keys.  This keyformatter will need to be passed in to override the default
-  /// formatter in print functions.
-  ///
-  /// Checks for LabeledSymbol, Symbol and then plain keys, in order.
-  static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter;
+/// The default KeyFormatter, which is used if no KeyFormatter is passed to
+/// a nonlinear 'print' function.  Automatically detects plain integer keys
+/// and Symbol keys.
+static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
 
-  /// Useful typedefs for operations with Values - allow for matlab interfaces
-  typedef FastList<Key> KeyList;
-  typedef FastVector<Key> KeyVector;
-  typedef FastSet<Key> KeySet;
-  typedef FastMap<Key,int> KeyGroupMap;
+// Helper function for Multi-robot Key Formatter
+GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
 
-  /// Utility function to print sets of keys with optional prefix
-  GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+///
+/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
+/// integer keys.  This keyformatter will need to be passed in to override the default
+/// formatter in print functions.
+///
+/// Checks for LabeledSymbol, Symbol and then plain keys, in order.
+static const gtsam::KeyFormatter MultiRobotKeyFormatter =
+    &_multirobotKeyFormatter;
 
-  /// Utility function to print sets of keys with optional prefix
-  GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+/// Useful typedef for operations with Values - allows for matlab interface
+typedef FastVector<Key> KeyVector;
 
-  /// Utility function to print sets of keys with optional prefix
-  GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
-}
+// TODO(frank): Nothing fast about these :-(
+typedef FastList<Key> KeyList;
+typedef FastSet<Key> KeySet;
+typedef FastMap<Key, int> KeyGroupMap;
+
+/// Utility function to print one key with optional prefix
+GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s =
+    "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<Key> {
+  static void Print(const Key& key, const std::string& str = "") {
+    PrintKey(key, str);
+  }
+  static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) {
+    return key1 == key2;
+  }
+};
+
+} // namespace gtsam
 
diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h
index 7299d7c8a..06e5ddeec 100644
--- a/gtsam/inference/MetisIndex-inl.h
+++ b/gtsam/inference/MetisIndex-inl.h
@@ -26,8 +26,8 @@ namespace gtsam {
 /* ************************************************************************* */
 template<class FACTOR>
 void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
-  std::map<int32_t, FastSet<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with  adjMap.first
-  std::map<int32_t, FastSet<int32_t> >::iterator iAdjMapIt;
+  std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with  adjMap.first
+  std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
   std::set<Key> keySet;
 
   /* ********** Convert to CSR format ********** */
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index 9de3fb66a..2d95442de 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -149,7 +149,7 @@ namespace gtsam {
     /// Return a natural Ordering. Typically used by iterative solvers
     template <class FACTOR>
     static Ordering Natural(const FactorGraph<FACTOR> &fg) {
-      FastSet<Key> src = fg.keys();
+      KeySet src = fg.keys();
       std::vector<Key> keys(src.begin(), src.end());
       std::stable_sort(keys.begin(), keys.end());
       return Ordering(keys);
diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index d4b8eeacf..f395fd4ab 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -17,14 +17,11 @@
 
 #pragma once
 
-#include <gtsam/base/FastList.h>
+#include <gtsam/inference/Key.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/base/FastVector.h>
-#include <gtsam/base/types.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/dllexport.h>
 
-#include <boost/none.hpp>
 #include <boost/optional/optional.hpp>
 #include <boost/smart_ptr/shared_ptr.hpp>
 
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index cd60a3eb9..a39b1d91e 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -47,7 +47,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
-    FastSet<Key> keys;
+    KeySet keys;
     BOOST_FOREACH(const sharedFactor& factor, *this)
     if (factor)
       keys.insert(factor->begin(), factor->end());
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 832114ff6..02bc95511 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -135,7 +135,7 @@ namespace gtsam {
      * Return the set of variables involved in the factors (computes a set
      * union).
      */
-    typedef FastSet<Key> Keys;
+    typedef KeySet Keys;
     Keys keys() const;
 
     /* return a map of (Key, dimension) */
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 41ee52b3a..6df1c8b10 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -84,11 +84,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u
 }
 
 /* ************************************************************************* */
-void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
+void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
                                   Values& theta, VariableIndex& variableIndex,
                                   VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
-                                  FastSet<Key>& replacedKeys, Base::Nodes& nodes,
-                                  FastSet<Key>& fixedVariables)
+                                  KeySet& replacedKeys, Base::Nodes& nodes,
+                                  KeySet& fixedVariables)
 {
   variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
   BOOST_FOREACH(Key key, unusedKeys) {
@@ -103,10 +103,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVect
 }
 
 /* ************************************************************************* */
-FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
+KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
     const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
 {
-  FastSet<Key> relinKeys;
+  KeySet relinKeys;
 
   if(const double* threshold = boost::get<double>(&relinearizeThreshold))
   {
@@ -131,7 +131,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
 }
 
 /* ************************************************************************* */
-void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double threshold,
+void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
                                          const VectorValues& delta, const ISAM2Clique::shared_ptr& clique)
 {
   // Check the current clique for relinearization
@@ -153,7 +153,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double thresho
 }
 
 /* ************************************************************************* */
-void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<char,Vector>& thresholds,
+void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vector>& thresholds,
                                       const VectorValues& delta,
                                       const ISAM2Clique::shared_ptr& clique)
 {
@@ -185,11 +185,11 @@ void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<cha
 }
 
 /* ************************************************************************* */
-FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
+KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
                                                         const VectorValues& delta,
                                                         const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
 {
-  FastSet<Key> relinKeys;
+  KeySet relinKeys;
   BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
     if(relinearizeThreshold.type() == typeid(double))
       CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
@@ -200,7 +200,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sh
 }
 
 /* ************************************************************************* */
-void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask)
+void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask)
 {
   static const bool debug = false;
   // does the separator contain any of the variables?
@@ -224,7 +224,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, co
 
 /* ************************************************************************* */
 void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
-    const FastSet<Key>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
+    const KeySet& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
 {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
@@ -272,7 +272,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
 
 /* ************************************************************************* */
 size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
-    const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
+    const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) {
 
   size_t lastBacksubVariableCount;
 
@@ -300,7 +300,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
 
 /* ************************************************************************* */
 namespace internal {
-void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys,
+void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& replacedKeys,
     const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) {
 
   // Check if any frontal or separator keys were recalculated, if so, we need
@@ -344,7 +344,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Ke
 }
 
 /* ************************************************************************* */
-size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
+size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
     const VectorValues& gradAtZero, VectorValues& RgProd) {
 
   // Update variables
diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h
index a8d03df13..d34480144 100644
--- a/gtsam/nonlinear/ISAM2-impl.h
+++ b/gtsam/nonlinear/ISAM2-impl.h
@@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl {
   /**
    * Remove variables from the ISAM2 system.
    */
-  static void RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
+  static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
     Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
-    VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
-    FastSet<Key>& fixedVariables);
+    VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
+    KeySet& fixedVariables);
 
   /**
    * Find the set of variables to be relinearized according to relinearizeThreshold.
@@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta,
+  static KeySet CheckRelinearizationFull(const VectorValues& delta,
       const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
 
   /**
@@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Key> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
+  static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
     const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
 
   /**
@@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
    *
    * Alternatively could we trace up towards the root for each variable here?
    */
-  static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask);
+  static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);
 
   /**
    * Apply expmap to the given values, but only for indices appearing in
@@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
    * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
   static void ExpmapMasked(Values& values, const VectorValues& delta,
-      const FastSet<Key>& mask,
+      const KeySet& mask,
       boost::optional<VectorValues&> invalidateIfDebug = boost::none,
       const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
@@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
    * Update the Newton's method step point, using wildfire
    */
   static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
-      const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
+      const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold);
 
   /**
    * Update the RgProd (R*g) incrementally taking into account which variables
    * have been recalculated in \c replacedKeys.  Only used in Dogleg.
    */
-  static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
+  static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
       const VectorValues& gradAtZero, VectorValues& RgProd);
   
   /**
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index f7c6d0345..538c66068 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
 namespace internal {
 template<class CLIQUE>
 void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
-    FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
+    KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
 {
   // if none of the variables in this clique (frontal and separator!) changed
   // significantly, then by the running intersection property, none of the
@@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
 
 template<class CLIQUE>
 bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
-    FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
+    KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
 {
   // if none of the variables in this clique (frontal and separator!) changed
   // significantly, then by the running intersection property, none of the
@@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
 
 /* ************************************************************************* */
 template<class CLIQUE>
-size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta) {
-  FastSet<Key> changed;
+size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta) {
+  KeySet changed;
   int count = 0;
   // starting from the root, call optimize on each conditional
   if(root)
@@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
 
 /* ************************************************************************* */
 template<class CLIQUE>
-size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta)
+size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta)
 {
-  FastSet<Key> changed;
+  KeySet changed;
   size_t count = 0;
 
   if (root) {
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 00ffdccef..f7ef09773 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -194,7 +194,7 @@ FastSet<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
 // (note that the remaining stuff is summarized in the cached factors)
 
 GaussianFactorGraph::shared_ptr
-ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const
+ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const
 {
   gttic(getAffectedFactors);
   FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
@@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastS
 
   gttic(affectedKeysSet);
   // for fast lookup below
-  FastSet<Key> affectedKeysSet;
+  KeySet affectedKeysSet;
   affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
   gttoc(affectedKeysSet);
 
@@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
 }
 
 /* ************************************************************************* */
-boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
+boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
                                                     const vector<Key>& observedKeys,
-                                                    const FastSet<Key>& unusedIndices,
+                                                    const KeySet& unusedIndices,
                                                     const boost::optional<FastMap<Key,int> >& constrainKeys,
                                                     ISAM2Result& result)
 {
@@ -326,7 +326,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
     affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
   gttoc(affectedKeys);
 
-  boost::shared_ptr<FastSet<Key> > affectedKeysSet(new FastSet<Key>()); // Will return this result
+  boost::shared_ptr<KeySet > affectedKeysSet(new KeySet()); // Will return this result
 
   if(affectedKeys.size() >= theta_.size() * batchThreshold)
   {
@@ -566,17 +566,17 @@ ISAM2Result ISAM2::update(
   variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors);
 
   // Compute unused keys and indices
-  FastSet<Key> unusedKeys;
-  FastSet<Key> unusedIndices;
+  KeySet unusedKeys;
+  KeySet unusedIndices;
   {
     // Get keys from removed factors and new factors, and compute unused keys,
     // i.e., keys that are empty now and do not appear in the new factors.
-    FastSet<Key> removedAndEmpty;
+    KeySet removedAndEmpty;
     BOOST_FOREACH(Key key, removeFactors.keys()) {
       if(variableIndex_[key].empty())
         removedAndEmpty.insert(removedAndEmpty.end(), key);
     }
-    FastSet<Key> newFactorSymbKeys = newFactors.keys();
+    KeySet newFactorSymbKeys = newFactors.keys();
     std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
       newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
 
@@ -602,10 +602,10 @@ ISAM2Result ISAM2::update(
 
   gttic(gather_involved_keys);
   // 3. Mark linear update
-  FastSet<Key> markedKeys = newFactors.keys(); // Get keys from new factors
+  KeySet markedKeys = newFactors.keys(); // Get keys from new factors
   // Also mark keys involved in removed factors
   {
-    FastSet<Key> markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
+    KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
     markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
   }
   // Also mark any provided extra re-eliminate keys
@@ -632,7 +632,7 @@ ISAM2Result ISAM2::update(
   gttoc(gather_involved_keys);
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
-  FastSet<Key> relinKeys;
+  KeySet relinKeys;
   if (relinearizeThisStep) {
     gttic(gather_relinearize_keys);
     // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
@@ -659,7 +659,7 @@ ISAM2Result ISAM2::update(
         result.detail->variableStatus[key].isRelinearized = true; } }
 
     // Add the variables being relinearized to the marked keys
-    FastSet<Key> markedRelinMask;
+    KeySet markedRelinMask;
     BOOST_FOREACH(const Key key, relinKeys)
       markedRelinMask.insert(key);
     markedKeys.insert(relinKeys.begin(), relinKeys.end());
@@ -674,7 +674,7 @@ ISAM2Result ISAM2::update(
 
       // Relin involved keys for detailed results
       if(params_.enableDetailedResults) {
-        FastSet<Key> involvedRelinKeys;
+        KeySet involvedRelinKeys;
         BOOST_FOREACH(const sharedClique& root, roots_)
           Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
         BOOST_FOREACH(Key key, involvedRelinKeys) {
@@ -726,7 +726,7 @@ ISAM2Result ISAM2::update(
 
   gttic(recalculate);
   // 8. Redo top of Bayes tree
-  boost::shared_ptr<FastSet<Key> > replacedKeys;
+  boost::shared_ptr<KeySet > replacedKeys;
   if(!markedKeys.empty() || !observedKeys.empty())
     replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result);
 
@@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
                               boost::optional<std::vector<size_t>&> deletedFactorsIndices)
 {
   // Convert to ordered set
-  FastSet<Key> leafKeys(leafKeysList.begin(), leafKeysList.end());
+  KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());
 
   // Keep track of marginal factors - map from clique to the marginal factors
   // that should be incorporated into it, passed up from it's children.
@@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
   FastSet<size_t> factorIndicesToRemove;
 
   // Keep track of variables removed in subtrees
-  FastSet<Key> leafKeysRemoved;
+  KeySet leafKeysRemoved;
 
   // Remove each variable and its subtrees
   BOOST_FOREACH(Key j, leafKeys) {
@@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
           graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
 
         // Reeliminate the linear graph to get the marginal and discard the conditional
-        const FastSet<Key> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
+        const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
         FastVector<Key> cliqueFrontalsToEliminate;
         std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
           std::back_inserter(cliqueFrontalsToEliminate));
@@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
     deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end());
 
   // Remove the marginalized variables
-  Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
+  Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
     deltaReplacedMask_, nodes_, fixedVariables_);
 }
 
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 5f5554e91..fdc0021e5 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -460,7 +460,7 @@ protected:
    * This is \c mutable because it is used internally to not update delta_
    * until it is needed.
    */
-  mutable FastSet<Key> deltaReplacedMask_; // TODO: Make sure accessed in the right way
+  mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way
 
   /** All original nonlinear factors are stored here to use during relinearization */
   NonlinearFactorGraph nonlinearFactors_;
@@ -476,7 +476,7 @@ protected:
 
   /** Set of variables that are involved with linear factors from marginalized
    * variables and thus cannot have their linearization points changed. */
-  FastSet<Key> fixedVariables_;
+  KeySet fixedVariables_;
 
   int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization
 
@@ -614,7 +614,7 @@ public:
   const VariableIndex& getVariableIndex() const { return variableIndex_; }
 
   /** Access the nonlinear variable index */
-  const FastSet<Key>& getFixedVariables() const { return fixedVariables_; }
+  const KeySet& getFixedVariables() const { return fixedVariables_; }
 
   size_t lastAffectedVariableCount;
   size_t lastAffectedFactorCount;
@@ -641,11 +641,11 @@ public:
 protected:
 
   FastSet<size_t> getAffectedFactors(const FastList<Key>& keys) const;
-  GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const;
+  GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
   GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
-  virtual boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
-      const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
+  virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
+      const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
   void updateDelta(bool forceFullSolve = false) const;
 
 }; // ISAM2
@@ -666,11 +666,11 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
 /// @return The number of variables that were solved for
 template<class CLIQUE>
 size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
-    double threshold, const FastSet<Key>& replaced, VectorValues& delta);
+    double threshold, const KeySet& replaced, VectorValues& delta);
 
 template<class CLIQUE>
 size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
-    double threshold, const FastSet<Key>& replaced, VectorValues& delta);
+    double threshold, const KeySet& replaced, VectorValues& delta);
 
 /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
 template<class CLIQUE>
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index f23418d2a..298ccf4b7 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -70,7 +70,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   stm << "  size=\"" << formatting.figureWidthInches << "," <<
     formatting.figureHeightInches << "\";\n\n";
 
-  FastSet<Key> keys = this->keys();
+  KeySet keys = this->keys();
 
   // Local utility function to extract x and y coordinates
   struct { boost::optional<Point2> operator()(
@@ -144,7 +144,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
   if (formatting.mergeSimilarFactors) {
     // Remove duplicate factors
-    FastSet<vector<Key> > structure;
+    std::set<vector<Key> > structure;
     BOOST_FOREACH(const sharedFactor& factor, *this){
       if(factor) {
         vector<Key> factorKeys = factor->keys();
@@ -234,8 +234,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
 }
 
 /* ************************************************************************* */
-FastSet<Key> NonlinearFactorGraph::keys() const {
-  FastSet<Key> keys;
+KeySet NonlinearFactorGraph::keys() const {
+  KeySet keys;
   BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
     if(factor)
       keys.insert(factor->begin(), factor->end());
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 169d12455..ecd3b9b56 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -106,7 +106,7 @@ namespace gtsam {
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /** return keys as an ordered set - ordering is by key value */
-    FastSet<Key> keys() const;
+    KeySet keys() const;
 
     /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
     double error(const Values& c) const;
diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp
index efdef9d44..6457e45fe 100644
--- a/gtsam/slam/tests/testRegularHessianFactor.cpp
+++ b/gtsam/slam/tests/testRegularHessianFactor.cpp
@@ -33,6 +33,7 @@ using namespace boost::assign;
 TEST(RegularHessianFactor, Constructors)
 {
   // First construct a regular JacobianFactor
+  // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I]
   Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
   Vector2 b(1,2);
   vector<pair<Key, Matrix> > terms;
@@ -42,6 +43,9 @@ TEST(RegularHessianFactor, Constructors)
   // Test conversion from JacobianFactor
   RegularHessianFactor<2> factor(jf);
 
+  // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b
+  // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f
+  // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5
   Matrix G11 = I_2x2;
   Matrix G12 = I_2x2;
   Matrix G13 = I_2x2;
@@ -53,7 +57,7 @@ TEST(RegularHessianFactor, Constructors)
 
   Vector2 g1 = b, g2 = b, g3 = b;
 
-  double f = 10;
+  double f = 5;
 
   // Test ternary constructor
   RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h
index 56850e991..c917b322e 100644
--- a/gtsam/symbolic/SymbolicFactor-inst.h
+++ b/gtsam/symbolic/SymbolicFactor-inst.h
@@ -38,7 +38,7 @@ namespace gtsam
       EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
     {
       // Gather all keys
-      FastSet<Key> allKeys;
+      KeySet allKeys;
       BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
         allKeys.insert(factor->begin(), factor->end());
       }
@@ -50,7 +50,7 @@ namespace gtsam
       }
 
       // Sort frontal keys
-      FastSet<Key> frontals(keys);
+      KeySet frontals(keys);
       const size_t nFrontals = keys.size();
 
       // Build a key vector with the frontals followed by the separator
diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
index b579364b6..37a4fe5a4 100644
--- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
+++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
@@ -193,7 +193,7 @@ private:
 
       // add the belief factor for each neighbor variable to this star graph
       // also record the factor index for later modification
-      FastSet<Key> neighbors = star->keys();
+      KeySet neighbors = star->keys();
       neighbors.erase(key);
       CorrectedBeliefIndices correctedBeliefIndices;
       BOOST_FOREACH(Key neighbor, neighbors) {
diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h
index f7a575f8c..68713f965 100644
--- a/gtsam_unstable/linear/QPSolver.h
+++ b/gtsam_unstable/linear/QPSolver.h
@@ -48,7 +48,7 @@ class QPSolver {
   GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process.
   VariableIndex costVariableIndex_, equalityVariableIndex_,
       inequalityVariableIndex_;
-  FastSet<Key> constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
+  KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
 
 public:
   /// Constructor
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
index 563da4a9f..9d4249af4 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
@@ -463,7 +463,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
 }
 
 /* ************************************************************************* */
-void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys,
+void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys,
     const std::string& label) {
   std::cout << label;
   BOOST_FOREACH(gtsam::Key key, keys) {
@@ -531,13 +531,13 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
         "BatchFixedLagSmoother::calculateMarginalFactors  Marginalize Keys: ");
 
   // Get the set of all keys involved in the factor graph
-  FastSet<Key> allKeys(graph.keys());
+  KeySet allKeys(graph.keys());
   if (debug)
     PrintKeySet(allKeys,
         "BatchFixedLagSmoother::calculateMarginalFactors  All Keys: ");
 
   // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
-  FastSet<Key> remainingKeys;
+  KeySet remainingKeys;
   std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
       marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
   if (debug)
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
index 89da3d7e5..605f3a5e8 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
@@ -161,7 +161,7 @@ protected:
 private:
   /** Private methods for printing debug information */
   static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
-  static void PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label);
+  static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
   static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
   static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
   static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index fcaae9626..1d913d61f 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
   if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize  ", "Updated Smoother Summarization:", DefaultKeyFormatter); }
 
   // Find the set of new separator keys
-  FastSet<Key> newSeparatorKeys;
+  KeySet newSeparatorKeys;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
     newSeparatorKeys.insert(key_value.key);
   }
@@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   }
 
   // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
-  FastSet<Key> newSeparatorKeys = removedFactors.keys();
+  KeySet newSeparatorKeys = removedFactors.keys();
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
     newSeparatorKeys.insert(key_value.key);
   }
@@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize  ", "New Separator Keys:", DefaultKeyFormatter); }
 
   // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
-  FastSet<Key> shortcutKeys = newSeparatorKeys;
+  KeySet shortcutKeys = newSeparatorKeys;
   BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
     shortcutKeys.insert(key);
   }
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
index 0f6515056..14c312f9d 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
@@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
   }
 
   // Get the set of separator keys
-  gtsam::FastSet<Key> separatorKeys;
+  gtsam::KeySet separatorKeys;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
     separatorKeys.insert(key_value.key);
   }
diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
index 3b6b884f6..b893860cc 100644
--- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
@@ -52,16 +52,16 @@ namespace internal {
 
 /* ************************************************************************* */
 NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
-    const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
+    const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
 
 
   // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
-  FastSet<Key> rootKeys;
-  FastSet<Key> allKeys(graph.keys());
+  KeySet rootKeys;
+  KeySet allKeys(graph.keys());
   std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));
 
   // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
-  FastSet<Key> marginalizeKeys;
+  KeySet marginalizeKeys;
   std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
 
   if(marginalizeKeys.size() == 0) {
diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
index fb4b78fc2..b8a9941ad 100644
--- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
+++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
@@ -152,7 +152,7 @@ namespace internal {
   /** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors.
    * Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs.  */
   NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
-      const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
+      const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
 
 }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index 9742aefd7..cf3155602 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
   previousSmootherSummarization_ = smootherSummarization;
 
   // Find the set of new separator keys
-  const FastSet<Key>& newSeparatorKeys = isam2_.getFixedVariables();
+  const KeySet& newSeparatorKeys = isam2_.getFixedVariables();
 
   // Use the shortcut to calculate an updated marginal on the current separator
   // Combine just the shortcut and the previousSmootherSummarization
@@ -312,7 +312,7 @@ std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2
 void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) {
 
   // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
-  FastSet<Key> shortcutKeys;
+  KeySet shortcutKeys;
   BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
     const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
     if(factor) {
@@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
   // variables that result from marginalizing out all of the other variables
 
   // Find the set of current separator keys
-  const FastSet<Key>& separatorKeys = isam2_.getFixedVariables();
+  const KeySet& separatorKeys = isam2_.getFixedVariables();
 
   // Find all cliques that contain any separator variables
   std::set<ISAM2Clique::shared_ptr> separatorCliques;
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
index 16a336695..b87409aae 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
@@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
   }
 
   // Get the set of separator keys
-  gtsam::FastSet<Key> separatorKeys;
+  gtsam::KeySet separatorKeys;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
     separatorKeys.insert(key_value.key);
   }
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
index 51c2a55a2..558654367 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
@@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   ordering = smoother.getOrdering();  // I'm really hoping this is an acceptable ordering...
   GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
 
-  FastSet<Key> eliminateKeys = linearFactors->keys();
+  KeySet eliminateKeys = linearFactors->keys();
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
     eliminateKeys.erase(key_value.key);
   }
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index 5f91527e6..22dd0ccaa 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
 //  GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
-  FastSet<Key> allkeys = LinFactorGraph->keys();
+  KeySet allkeys = LinFactorGraph->keys();
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
     allkeys.erase(key_value.key);
   }
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index 9708eedb5..c372577ca 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
 //  GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
-  FastSet<Key> allkeys = LinFactorGraph->keys();
+  KeySet allkeys = LinFactorGraph->keys();
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
     allkeys.erase(key_value.key);
   std::vector<Key> variables(allkeys.begin(), allkeys.end());
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index da60c2c31..f1487f562 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -70,7 +70,7 @@ namespace gtsam {
      * @param body_P_sensor is the transform from body to sensor frame (default identity)
      */
     MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
-        FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
+        KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
         boost::optional<POSE> body_P_sensor = boost::none) :
           Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
           throwCheirality_(false), verboseCheirality_(false) {
@@ -91,7 +91,7 @@ namespace gtsam {
      * @param body_P_sensor is the transform from body to sensor frame  (default identity)
      */
     MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
-        FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
+        KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
         bool throwCheirality, bool verboseCheirality,
         boost::optional<POSE> body_P_sensor = boost::none) :
           Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
index 2794e5707..ca91d4cb5 100644
--- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
+++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
@@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){
   n_measPixel << 10, 10, 10, 10, 10, 10;
   const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
 
-  FastSet<Key> views;
+  KeySet views;
   views.insert(x1);
   views.insert(x2);
   views.insert(x3);
diff --git a/matlab.h b/matlab.h
index 349cdeea4..a215c6e07 100644
--- a/matlab.h
+++ b/matlab.h
@@ -71,16 +71,16 @@ FastVector<Key> createKeyVector(string s, const Vector& I) {
 }
 
 // Create a KeySet from indices
-FastSet<Key> createKeySet(const Vector& I) {
-  FastSet<Key> set;
+KeySet createKeySet(const Vector& I) {
+  KeySet set;
   for (int i = 0; i < I.size(); i++)
     set.insert(I[i]);
   return set;
 }
 
 // Create a KeySet from indices using symbol
-FastSet<Key> createKeySet(string s, const Vector& I) {
-  FastSet<Key> set;
+KeySet createKeySet(string s, const Vector& I) {
+  KeySet set;
   char c = s[0];
   for (int i = 0; i < I.size(); i++)
     set.insert(symbol(c, I[i]));
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 4e14d49b3..7922c14d1 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -186,12 +186,12 @@ done:
 //  Permuted<VectorValues> permuted(permutation, values);
 //
 //  // After permutation, the indices above the threshold are 2 and 2
-//  FastSet<Key> expected;
+//  KeySet expected;
 //  expected.insert(2);
 //  expected.insert(3);
 //
 //  // Indices checked by CheckRelinearization
-//  FastSet<Key> actual = Impl::CheckRelinearization(permuted, 0.1);
+//  KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
 //
 //  EXPECT(assert_equal(expected, actual));
 //}
diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp
index 0af4c4a57..bd202e991 100644
--- a/tests/testMarginals.cpp
+++ b/tests/testMarginals.cpp
@@ -226,7 +226,7 @@ TEST(Marginals, order) {
     vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
 
   Marginals marginals(fg, vals);
-  FastSet<Key> set = fg.keys();
+  KeySet set = fg.keys();
   FastVector<Key> keys(set.begin(), set.end());
   JointMarginal joint = marginals.jointMarginalCovariance(keys);
 
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index f398a33fe..b81a3113b 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error )
 TEST( NonlinearFactorGraph, keys )
 {
   NonlinearFactorGraph fg = createNonlinearFactorGraph();
-  FastSet<Key> actual = fg.keys();
+  KeySet actual = fg.keys();
   LONGS_EQUAL(3, (long)actual.size());
-  FastSet<Key>::const_iterator it = actual.begin();
+  KeySet::const_iterator it = actual.begin();
   LONGS_EQUAL((long)L(1), (long)*(it++));
   LONGS_EQUAL((long)X(1), (long)*(it++));
   LONGS_EQUAL((long)X(2), (long)*(it++));