diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index e81565e65..63c52ad5f 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -66,10 +66,10 @@ namespace gtsam { /// @{ /** Default constructor */ - BayesTreeCliqueBase() {} + BayesTreeCliqueBase() : problemSize_(1) {} /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional) {} + BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} /// @} @@ -80,6 +80,7 @@ namespace gtsam { sharedConditional conditional_; derived_weak_ptr parent_; FastVector children; + int problemSize_; /// Fill the elimination result produced during elimination. Here this just stores the /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique @@ -114,6 +115,9 @@ namespace gtsam { /** return a shared_ptr to the parent clique */ derived_ptr parent() const { return parent_.lock(); } + /** Problem size (used for parallel traversal) */ + int problemSize() const { return problemSize_; } + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 401ba174e..298bf6389 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -156,7 +156,9 @@ namespace gtsam { EliminationData eliminationPreOrderVisitor( const typename JUNCTIONTREE::sharedNode& node, EliminationData& parentData) { - return EliminationData(&parentData, node->children.size()); + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 77e63ba7b..11967e858 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -63,7 +63,7 @@ namespace gtsam { size_t oldSize = values_.size(); hint = values_.insert(hint, key_value); if(values_.size() > oldSize) { - values_.erase(hint); + values_.unsafe_erase(hint); throw std::out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); } else { hint->second = key_value.second; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 125cc8060..65c385d11 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -24,6 +24,11 @@ #include +#include +#undef min +#undef max +#undef ERROR + namespace gtsam { /** @@ -88,14 +93,14 @@ namespace gtsam { class GTSAM_EXPORT VectorValues { protected: typedef VectorValues This; - typedef FastMap Values; ///< Typedef for the collection of Vectors making up a VectorValues + typedef tbb::concurrent_unordered_map Values; ///< Typedef for the collection of Vectors making up a VectorValues Values values_; ///< Collection of Vectors making up this VectorValues public: typedef Values::iterator iterator; ///< Iterator over vector values typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values + //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair @@ -207,7 +212,7 @@ namespace gtsam { /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { - if(values_.erase(var) == 0) + if(values_.unsafe_erase(var) == 0) throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); } @@ -218,10 +223,10 @@ namespace gtsam { const_iterator begin() const { return values_.begin(); } ///< Iterator over variables iterator end() { return values_.end(); } ///< Iterator over variables const_iterator end() const { return values_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables + //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ iterator find(Key j) { return values_.find(j); } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 29e1e1096..4ec8c9a07 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -132,7 +132,8 @@ namespace gtsam //return rootData.results; OptimizeData rootData; OptimizeClique preVisitor; - treeTraversal::DepthFirstForest(bayesTree, rootData, preVisitor); + treeTraversal::no_op postVisitor; + treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor); return preVisitor.collectedResult; } }