diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 4bbed8bf0..ce8f67a1f 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -416,7 +416,7 @@ namespace gtsam {
       child->parent_ = typename Clique::weak_ptr();
 
     BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
-      nodes_.erase(j);
+      nodes_.unsafe_erase(j);
     }
   }
 
@@ -495,7 +495,7 @@ namespace gtsam {
 
       // Remove this node from the nodes index
       BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
-        nodes_.erase(j); }
+        nodes_.unsafe_erase(j); }
 
       // Erase the parent and children pointers
       (*clique)->parent_.reset();
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 15c572128..bd3b7db6a 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -21,6 +21,10 @@
 
 #include <vector>
 #include <string>
+#include <tbb/tbb.h>
+#undef max
+#undef min
+#undef ERROR
 
 #include <gtsam/base/types.h>
 #include <gtsam/base/FastList.h>
@@ -87,7 +91,7 @@ namespace gtsam {
     typedef FastList<sharedClique> Cliques;
 
     /** Map from keys to Clique */
-    typedef FastMap<Key, sharedClique> Nodes;
+    typedef tbb::concurrent_unordered_map<Key, sharedClique> Nodes;
 
   protected:
 
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index e380c5c38..c5a995d56 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -23,6 +23,10 @@
 
 #include <vector>
 #include <boost/make_shared.hpp>
+#include <tbb/tbb.h>
+#undef max
+#undef min
+#undef ERROR
 
 namespace gtsam {
 
@@ -51,7 +55,7 @@ namespace gtsam {
     : slot(_slot), dimension(_dimension) {}
     std::string toString() const;
   };
-  class Scatter : public FastMap<Key, SlotEntry> {
+  class Scatter : public std::map<Key, SlotEntry, std::less<Key>, tbb::tbb_allocator<std::pair<const Key, SlotEntry> > > {
   public:
     Scatter() {}
     Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering = boost::none);
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 087acdee9..6325fae03 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -55,7 +55,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const std::vec
     deltaNewton.erase(key);
     RgProd.erase(key);
     replacedKeys.erase(key);
-    nodes.erase(key);
+    nodes.unsafe_erase(key);
     theta.erase(key);
     fixedVariables.erase(key);
   }
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 55aab98f5..618b5b878 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -764,7 +764,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
 
   // Remove each variable and its subtrees
   BOOST_REVERSE_FOREACH(Key j, leafKeys) {
-    if(nodes_.exists(j)) { // If the index was not already removed by removing another subtree
+    if(nodes_.count(j)) { // If the index was not already removed by removing another subtree
       sharedClique clique = nodes_[j];
 
       // See if we should remove the whole clique