From 027a4a5a8e03331fe9fd85c1f811e68ab77b58cc Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 5 Apr 2022 00:00:53 -0400
Subject: [PATCH 1/2] Fix the race!

---
 gtsam/inference/ClusterTree-inst.h | 34 ++++++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index b042c0c8e3..98cc613119 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@@ -15,6 +15,10 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
 
+#ifdef GTSAM_USE_TBB
+#include <mutex>
+#endif
+
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -120,20 +124,39 @@ struct EliminationData {
   size_t myIndexInParent;
   FastVector<sharedFactor> childFactors;
   boost::shared_ptr<BTNode> bayesTreeNode;
+#ifdef GTSAM_USE_TBB
+  boost::shared_ptr<std::mutex> writeLock;
+#endif
 
   EliminationData(EliminationData* _parentData, size_t nChildren) :
-      parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
+      parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>())
+#ifdef GTSAM_USE_TBB
+      , writeLock(boost::make_shared<std::mutex>())
+#endif
+    {
     if (parentData) {
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->lock();
+#endif
       myIndexInParent = parentData->childFactors.size();
       parentData->childFactors.push_back(sharedFactor());
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->unlock();
+#endif
     } else {
       myIndexInParent = 0;
     }
     // Set up BayesTree parent and child pointers
     if (parentData) {
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->lock();
+#endif
       if (parentData->parentData) // If our parent is not the dummy node
         bayesTreeNode->parent_ = parentData->bayesTreeNode;
       parentData->bayesTreeNode->children.push_back(bayesTreeNode);
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->unlock();
+#endif
     }
   }
 
@@ -196,8 +219,15 @@ struct EliminationData {
         nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
 
       // Store remaining factor in parent's gathered factors
-      if (!eliminationResult.second->empty())
+      if (!eliminationResult.second->empty()) {
+#ifdef GTSAM_USE_TBB
+        myData.parentData->writeLock->lock();
+#endif
         myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
+#ifdef GTSAM_USE_TBB
+        myData.parentData->writeLock->unlock();
+#endif
+      }
     }
   };
 };

From af1dedb879778277dc9fd0759b3defd554d4c947 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 5 Apr 2022 23:25:09 -0400
Subject: [PATCH 2/2] Optimize locking

---
 gtsam/inference/ClusterTree-inst.h | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index 98cc613119..9bc1419558 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@@ -148,15 +148,9 @@ struct EliminationData {
     }
     // Set up BayesTree parent and child pointers
     if (parentData) {
-#ifdef GTSAM_USE_TBB
-      parentData->writeLock->lock();
-#endif
       if (parentData->parentData) // If our parent is not the dummy node
         bayesTreeNode->parent_ = parentData->bayesTreeNode;
       parentData->bayesTreeNode->children.push_back(bayesTreeNode);
-#ifdef GTSAM_USE_TBB
-      parentData->writeLock->unlock();
-#endif
     }
   }