Merge pull request #460 from Cyberdr8gon/develop

Support thread-safe const marginalCovariance

gtsam/inference/BayesTreeCliqueBase-inst.h

@@ -91,6 +91,7 @@ namespace gtsam {
   template<class DERIVED, class FACTORGRAPH>
   size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
   {
+    std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
     if (!cachedSeparatorMarginal_)
       return 0;
 
@@ -144,6 +145,7 @@ namespace gtsam {
   typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
   BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
       Eliminate function) const {
+    std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
     gttic(BayesTreeCliqueBase_separatorMarginal);
     // Check if the Separator marginal was already calculated
     if (!cachedSeparatorMarginal_) {
@@ -206,6 +208,8 @@ namespace gtsam {
     // When a shortcut is requested, all of the shortcuts between it and the
     // root are also generated. So, if this clique's cached shortcut is set,
     // recursively call over all child cliques. Otherwise, it is unnecessary.
+    
+    std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
     if (cachedSeparatorMarginal_) {
       for(derived_ptr& child: children) {
         child->deleteCachedShortcuts();

gtsam/inference/BayesTreeCliqueBase.h

@@ -24,6 +24,7 @@
 #include <boost/optional.hpp>
 
 #include <string>
+#include <mutex>
 
 namespace gtsam {
 
@@ -75,10 +76,28 @@ namespace gtsam {
     /** Construct from a conditional, leaving parent and child pointers uninitialized */
     BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {}
 
+    /** Shallow copy constructor */
+    BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {}
+
+    /** Shallow copy assignment constructor */
+    BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) {
+      conditional_ = c.conditional_;
+      parent_ = c.parent_;
+      children = c.children;
+      problemSize_ = c.problemSize_;
+      is_root = c.is_root;
+      return *this;
+    }
+
     /// @}
 
-    /// This stores the Cached separator margnal P(S)
+    /// This stores the Cached separator marginal P(S)
     mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
+    /// This protects Cached seperator marginal P(S) from concurrent read/writes
+    /// as many the functions which access it are const (hence the mutable)
+    /// leading to the false impression that these const functions are thread-safe
+    /// which is not true due to these mutable values. This is fixed by applying this mutex.
+    mutable std::mutex cachedSeparatorMarginalMutex_;
 
   public:
     sharedConditional conditional_;
@@ -144,7 +163,9 @@ namespace gtsam {
     void deleteCachedShortcuts();
 
     const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
-      return cachedSeparatorMarginal_; }
+      std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
+      return cachedSeparatorMarginal_; 
+    }
 
     friend class BayesTree<DerivedType>;
 
@@ -159,7 +180,10 @@ namespace gtsam {
     KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
 
     /** Non-recursive delete cached shortcuts and marginals - internal only. */
-    void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
+    void deleteCachedShortcutsNonRecursive() { 
+      std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
+      cachedSeparatorMarginal_ = boost::none; 
+    }
 
   private: