Switched a couple of data structures - ISAM2::Nodes and Scatter, to tbb versions for better performance

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();

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:
 

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);

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);
   }

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