diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index a02fe274e..dfcc7c318 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -91,6 +91,7 @@ namespace gtsam { template size_t BayesTreeCliqueBase::numCachedSeparatorMarginals() const { + std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); if (!cachedSeparatorMarginal_) return 0; @@ -144,6 +145,7 @@ namespace gtsam { typename BayesTreeCliqueBase::FactorGraphType BayesTreeCliqueBase::separatorMarginal( Eliminate function) const { + std::lock_guard 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 marginalLock(cachedSeparatorMarginalMutex_); if (cachedSeparatorMarginal_) { for(derived_ptr& child: children) { child->deleteCachedShortcuts(); diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 7aca84635..7b79ccf68 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -24,6 +24,7 @@ #include #include +#include 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 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& cachedSeparatorMarginal() const { - return cachedSeparatorMarginal_; } + std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); + return cachedSeparatorMarginal_; + } friend class BayesTree; @@ -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 marginalLock(cachedSeparatorMarginalMutex_); + cachedSeparatorMarginal_ = boost::none; + } private: