From bd89c5fd454e10e3295496dea6b4040f867cd65e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 15 Aug 2013 17:21:20 +0000 Subject: [PATCH] Using FastVector (with tbb allocator) instead of vector --- gtsam/base/SymmetricBlockMatrix.h | 3 ++- gtsam/base/VerticalBlockMatrix.h | 3 ++- gtsam/base/treeTraversal-inst.h | 7 ++++--- gtsam/inference/BayesTree-inst.h | 10 +++++----- gtsam/inference/BayesTree.h | 14 +++++++------- gtsam/inference/BayesTreeCliqueBase-inst.h | 16 ++++++++-------- gtsam/inference/BayesTreeCliqueBase.h | 7 ++++--- gtsam/inference/EliminationTree-inst.h | 16 ++++++++-------- gtsam/inference/EliminationTree.h | 15 ++++++++------- gtsam/inference/Factor.h | 12 ++++++------ gtsam/inference/FactorGraph.h | 9 +++++---- gtsam/inference/JunctionTree-inst.h | 6 +++--- gtsam/inference/JunctionTree.h | 15 ++++++++------- gtsam/inference/VariableSlots.h | 8 ++++---- gtsam/inference/inference-inst.h | 5 +++-- gtsam/linear/GaussianConditional.cpp | 8 ++++---- gtsam/linear/HessianFactor.cpp | 4 ++-- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor-inl.h | 4 ++-- gtsam/linear/JacobianFactor.cpp | 18 +++++++++--------- gtsam/linear/VectorValues.cpp | 4 ++-- gtsam/linear/VectorValues.h | 3 ++- gtsam/nonlinear/ISAM2-impl.cpp | 10 +++++----- gtsam/nonlinear/ISAM2-impl.h | 6 +++--- gtsam/nonlinear/ISAM2-inl.h | 6 +++--- gtsam/nonlinear/ISAM2.cpp | 8 ++++---- gtsam/symbolic/SymbolicFactor.cpp | 3 ++- 27 files changed, 116 insertions(+), 106 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 8b4db01eb..13b493f36 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -44,7 +45,7 @@ namespace gtsam { protected: Matrix matrix_; ///< The full matrix - std::vector variableColOffsets_; ///< the starting columns of each block (0-based) + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 7677e817e..0e45a8f0d 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -46,7 +47,7 @@ namespace gtsam { protected: Matrix matrix_; ///< The full matrix - std::vector variableColOffsets_; ///< the starting columns of each block (0-based) + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index a8896b45d..91d9eae74 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include @@ -143,7 +144,7 @@ namespace gtsam { // Set TBB ref count set_ref_count(1 + (int)roots.size()); // Create data and tasks for our children - std::vector tasks; + FastVector tasks; tasks.reserve(roots.size()); BOOST_FOREACH(const boost::shared_ptr& root, roots) { @@ -301,12 +302,12 @@ namespace gtsam { * return a collection of shared pointers to \c FOREST::Node. * @return The new collection of roots. */ template - std::vector > CloneForest(const FOREST& forest) + FastVector > CloneForest(const FOREST& forest) { typedef typename FOREST::Node Node; boost::shared_ptr rootContainer = boost::make_shared(); DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return std::vector >(rootContainer->children.begin(), rootContainer->children.end()); + return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); } diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 4bbed8bf0..7a88e37f6 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -340,7 +340,7 @@ namespace gtsam { // Get the set of variables to eliminate, which is C1\B. gttic(Full_root_factoring); boost::shared_ptr p_C1_B; { - std::vector C1_minus_B; { + FastVector C1_minus_B; { FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) { C1_minus_B_set.erase(j); } @@ -352,7 +352,7 @@ namespace gtsam { FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); } boost::shared_ptr p_C2_B; { - std::vector C2_minus_B; { + FastVector C2_minus_B; { FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) { C2_minus_B_set.erase(j); } @@ -401,12 +401,12 @@ namespace gtsam { void BayesTree::removeClique(sharedClique clique) { if (clique->isRoot()) { - typename std::vector::iterator root = std::find(roots_.begin(), roots_.end(), clique); + typename FastVector::iterator root = std::find(roots_.begin(), roots_.end(), clique); if(root != roots_.end()) roots_.erase(root); } else { // detach clique from parent sharedClique parent = clique->parent_.lock(); - typename std::vector::iterator child = std::find(parent->children.begin(), parent->children.end(), clique); + typename FastVector::iterator child = std::find(parent->children.begin(), parent->children.end(), clique); assert(child != parent->children.end()); parent->children.erase(child); } @@ -447,7 +447,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::removeTop(const std::vector& keys, BayesNetType& bn, Cliques& orphans) + void BayesTree::removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans) { // process each key of the new factor BOOST_FOREACH(const Key& j, keys) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 143a2f0f5..3050d93dd 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,7 +19,6 @@ #pragma once -#include #include #include #undef max @@ -29,6 +28,7 @@ #include #include #include +#include namespace gtsam { @@ -48,8 +48,8 @@ namespace gtsam { /** store all the sizes */ struct GTSAM_EXPORT BayesTreeCliqueData { - std::vector conditionalSizes; - std::vector separatorSizes; + FastVector conditionalSizes; + FastVector separatorSizes; BayesTreeCliqueStats getStats() const; }; @@ -91,7 +91,7 @@ namespace gtsam { typedef FastList Cliques; /** Map from keys to Clique */ - typedef FastMap Nodes; + typedef tbb::concurrent_unordered_map Nodes; protected: @@ -99,7 +99,7 @@ namespace gtsam { Nodes nodes_; /** Root cliques */ - std::vector roots_; + FastVector roots_; /// @name Standard Constructors /// @{ @@ -145,7 +145,7 @@ namespace gtsam { const sharedNode operator[](Key j) const { return nodes_.at(j); } /** return root cliques */ - const std::vector& roots() const { return roots_; } + const FastVector& roots() const { return roots_; } /** alternate syntax for matlab: find the clique that contains the variable with Index j */ const sharedClique& clique(Key j) const { @@ -215,7 +215,7 @@ namespace gtsam { * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - void removeTop(const std::vector& keys, BayesNetType& bn, Cliques& orphans); + void removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans); /** * Remove the requested subtree. */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index eb736f663..274886c21 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -41,12 +41,12 @@ namespace gtsam { /* ************************************************************************* */ template - std::vector + FastVector BayesTreeCliqueBase::separator_setminus_B(const derived_ptr& B) const { FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); - std::vector S_setminus_B; + FastVector S_setminus_B; std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); return S_setminus_B; @@ -54,14 +54,14 @@ namespace gtsam { /* ************************************************************************* */ template - std::vector BayesTreeCliqueBase::shortcut_indices( + FastVector BayesTreeCliqueBase::shortcut_indices( const derived_ptr& B, const FactorGraphType& p_Cp_B) const { gttic(shortcut_indices); FastSet allKeys = p_Cp_B.keys(); FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); - std::vector S_setminus_B = separator_setminus_B(B); - std::vector keep; + FastVector S_setminus_B = separator_setminus_B(B); + FastVector keep; // keep = S\B intersect allKeys (S_setminus_B is already sorted) std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // allKeys.begin(), allKeys.end(), back_inserter(keep)); @@ -114,7 +114,7 @@ namespace gtsam { gttic(BayesTreeCliqueBase_shortcut); // We only calculate the shortcut when this clique is not B // and when the S\B is not empty - std::vector S_setminus_B = separator_setminus_B(B); + FastVector S_setminus_B = separator_setminus_B(B); if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty()) { // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph @@ -125,7 +125,7 @@ namespace gtsam { p_Cp_B += parent->conditional_; // P(Fp|Sp) // Determine the variables we want to keepSet, S union B - std::vector keep = shortcut_indices(B, p_Cp_B); + FastVector keep = shortcut_indices(B, p_Cp_B); // Marginalize out everything except S union B boost::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); @@ -171,7 +171,7 @@ namespace gtsam { p_Cp += parent->conditional_; // P(Fp|Sp) // The variables we want to keepSet are exactly the ones in S - std::vector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); + FastVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); } } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 56f4744fe..e81565e65 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -78,7 +79,7 @@ namespace gtsam { public: sharedConditional conditional_; derived_weak_ptr parent_; - std::vector children; + FastVector children; /// Fill the elimination result produced during elimination. Here this just stores the /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique @@ -140,12 +141,12 @@ namespace gtsam { protected: /// Calculate set \f$ S \setminus B \f$ for shortcut calculations - std::vector separator_setminus_B(const derived_ptr& B) const; + FastVector separator_setminus_B(const derived_ptr& B) const; /** Determine variable indices to keep in recursive separator shortcut calculation The factor * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables * not in S union B. */ - std::vector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; + FastVector 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; } diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 617fb6e0e..70b0dd393 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -36,7 +36,7 @@ namespace gtsam { typename EliminationTree::sharedFactor EliminationTree::Node::eliminate( const boost::shared_ptr& output, - const Eliminate& function, const std::vector& childrenResults) const + const Eliminate& function, const FastVector& childrenResults) const { // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. @@ -49,7 +49,7 @@ namespace gtsam { gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); // Do dense elimination step - std::vector keyAsVector(1); keyAsVector[0] = key; + FastVector keyAsVector(1); keyAsVector[0] = key; std::pair, boost::shared_ptr > eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); @@ -90,10 +90,10 @@ namespace gtsam { static const size_t none = std::numeric_limits::max(); // Allocate result parent vector and vector of last factor columns - std::vector nodes(n); - std::vector parents(n, none); - std::vector prevCol(m, none); - std::vector factorUsed(m, false); + FastVector nodes(n); + FastVector parents(n, none); + FastVector prevCol(m, none); + FastVector factorUsed(m, false); try { // for column j \in 1 to n do @@ -192,7 +192,7 @@ namespace gtsam { boost::shared_ptr result = boost::make_shared(); // Run tree elimination algorithm - std::vector remainingFactors = inference::EliminateTree(result, *this, function); + FastVector remainingFactors = inference::EliminateTree(result, *this, function); // Add remaining factors that were not involved with eliminated variables boost::shared_ptr allRemainingFactors = boost::make_shared(); @@ -215,7 +215,7 @@ namespace gtsam { bool EliminationTree::equals(const This& expected, double tol) const { // Depth-first-traversal stacks - std::stack > stack1, stack2; + std::stack > stack1, stack2; // Add roots in sorted order { diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index fd19e0a65..b0f5acca3 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -21,6 +21,7 @@ #include #include +#include class EliminationTreeTester; // for unit tests, see testEliminationTree @@ -63,15 +64,15 @@ namespace gtsam { typedef typename GRAPH::Eliminate Eliminate; struct Node { - typedef std::vector Factors; - typedef std::vector > Children; + typedef FastVector Factors; + typedef FastVector > Children; Key key; ///< key associated with root Factors factors; ///< factors associated with root Children children; ///< sub-trees sharedFactor eliminate(const boost::shared_ptr& output, - const Eliminate& function, const std::vector& childrenFactors) const; + const Eliminate& function, const FastVector& childrenFactors) const; void print(const std::string& str, const KeyFormatter& keyFormatter) const; }; @@ -82,8 +83,8 @@ namespace gtsam { /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - std::vector roots_; - std::vector remainingFactors_; + FastVector roots_; + FastVector remainingFactors_; /// @name Standard Constructors /// @{ @@ -147,10 +148,10 @@ namespace gtsam { /// @{ /** Return the set of roots (one for a tree, multiple for a forest) */ - const std::vector& roots() const { return roots_; } + const FastVector& roots() const { return roots_; } /** Return the remaining factors that are not pulled into elimination */ - const std::vector& remainingFactors() const { return remainingFactors_; } + const FastVector& remainingFactors() const { return remainingFactors_; } /** Swap the data of this tree with another one, this operation is very fast. */ void swap(This& other); diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index c407c6507..73ab3607c 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -21,10 +21,10 @@ #pragma once -#include #include #include +#include #include namespace gtsam { @@ -58,15 +58,15 @@ namespace gtsam { public: /// Iterator over keys - typedef std::vector::iterator iterator; + typedef FastVector::iterator iterator; /// Const iterator over keys - typedef std::vector::const_iterator const_iterator; + typedef FastVector::const_iterator const_iterator; protected: /// The keys involved in this factor - std::vector keys_; + FastVector keys_; /// @name Standard Constructors /// @{ @@ -112,7 +112,7 @@ namespace gtsam { const_iterator find(Key key) const { return std::find(begin(), end(), key); } /// Access the factor's involved variable keys - const std::vector& keys() const { return keys_; } + const FastVector& keys() const { return keys_; } /** Iterator at beginning of involved variable keys */ const_iterator begin() const { return keys_.begin(); } @@ -148,7 +148,7 @@ namespace gtsam { /// @{ /** @return keys involved in this factor */ - std::vector& keys() { return keys_; } + FastVector& keys() { return keys_; } /** Iterator at beginning of involved variable keys */ iterator begin() { return keys_.begin(); } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0af0eb6b9..e8c10d532 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -30,6 +30,7 @@ #include #include +#include #include namespace gtsam { @@ -82,8 +83,8 @@ namespace gtsam { typedef FACTOR FactorType; ///< factor type typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef sharedFactor value_type; - typedef typename std::vector::iterator iterator; - typedef typename std::vector::const_iterator const_iterator; + typedef typename FastVector::iterator iterator; + typedef typename FastVector::const_iterator const_iterator; private: typedef FactorGraph This; ///< Typedef for this class @@ -94,7 +95,7 @@ namespace gtsam { GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) /** Collection of factors */ - std::vector factors_; + FastVector factors_; /// @name Standard Constructors /// @{ @@ -142,7 +143,7 @@ namespace gtsam { /** * Reserve space for the specified number of factors if you know in - * advance how many there will be (works like std::vector::reserve). + * advance how many there will be (works like FastVector::reserve). */ void reserve(size_t size) { factors_.reserve(size); } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index f805b4859..401ba174e 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -38,8 +38,8 @@ namespace gtsam { struct ConstructorTraversalData { ConstructorTraversalData* const parentData; typename JunctionTree::sharedNode myJTNode; - std::vector childSymbolicConditionals; - std::vector childSymbolicFactors; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} }; @@ -128,7 +128,7 @@ namespace gtsam { struct EliminationData { EliminationData* const parentData; size_t myIndexInParent; - std::vector childFactors; + FastVector childFactors; boost::shared_ptr bayesTreeNode; EliminationData(EliminationData* _parentData, size_t nChildren) : parentData(_parentData), diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index e7df4e32d..f07e52a31 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -61,9 +62,9 @@ namespace gtsam { typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine struct Node { - typedef std::vector Keys; - typedef std::vector Factors; - typedef std::vector > Children; + typedef FastVector Keys; + typedef FastVector Factors; + typedef FastVector > Children; Keys keys; ///< Frontal keys of this node Factors factors; ///< Factors associated with this node @@ -83,8 +84,8 @@ namespace gtsam { /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - std::vector roots_; - std::vector remainingFactors_; + FastVector roots_; + FastVector remainingFactors_; protected: @@ -127,10 +128,10 @@ namespace gtsam { /// @{ /** Return the set of roots (one for a tree, multiple for a forest) */ - const std::vector& roots() const { return roots_; } + const FastVector& roots() const { return roots_; } /** Return the remaining factors that are not pulled into elimination */ - const std::vector& remainingFactors() const { return remainingFactors_; } + const FastVector& remainingFactors() const { return remainingFactors_; } /// @} diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 7e80e86cd..4e23615bc 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -28,7 +29,6 @@ #include #include -#include namespace gtsam { @@ -50,11 +50,11 @@ namespace gtsam { * * \nosubgrouping */ -class VariableSlots : public FastMap > { +class VariableSlots : public FastMap > { public: - typedef FastMap > Base; + typedef FastMap > Base; /// @name Standard Constructors /// @{ @@ -106,7 +106,7 @@ VariableSlots::VariableSlots(const FG& factorGraph) // the array entry for each factor that will indicate the factor // does not involve the variable. iterator thisVarSlots; bool inserted; - boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector())); + boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, FastVector())); if(inserted) thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits::max()); thisVarSlots->second[jointFactorPos] = factorVarSlot; diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 1d4e7c0dc..d21098844 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { namespace inference { @@ -33,7 +34,7 @@ namespace gtsam { template struct EliminationData { EliminationData* const parentData; - std::vector childFactors; + FastVector childFactors; EliminationData(EliminationData* _parentData, size_t nChildren) : parentData(_parentData) { childFactors.reserve(nChildren); } }; @@ -71,7 +72,7 @@ namespace gtsam { * TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, * TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */ template - std::vector + FastVector EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function) { // Typedefs diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index e8dbbacb0..57d0a0982 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -113,7 +113,7 @@ namespace gtsam { VectorValues GaussianConditional::solve(const VectorValues& x) const { // Solve matrix - Vector xS = x.vector(vector(beginParents(), endParents())); + Vector xS = x.vector(FastVector(beginParents(), endParents())); xS = getb() - get_S() * xS; Vector soln = get_R().triangularView().solve(xS); @@ -135,8 +135,8 @@ namespace gtsam { VectorValues GaussianConditional::solveOtherRHS( const VectorValues& parents, const VectorValues& rhs) const { - Vector xS = parents.vector(vector(beginParents(), endParents())); - const Vector rhsR = rhs.vector(vector(beginFrontals(), endFrontals())); + Vector xS = parents.vector(FastVector(beginParents(), endParents())); + const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); xS = rhsR - get_S() * xS; Vector soln = get_R().triangularView().solve(xS); @@ -158,7 +158,7 @@ namespace gtsam { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { - Vector frontalVec = gy.vector(vector(beginFrontals(), endFrontals())); + Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); // Check for indeterminant solution diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 1fa94922b..63dd5d606 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -282,7 +282,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, gttic(allocate); // Allocate with dimensions for each variable plus 1 at the end for the information vector keys_.resize(scatter->size()); - vector dims(scatter->size() + 1); + FastVector dims(scatter->size() + 1); BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { keys_[key_slotentry.second.slot] = key_slotentry.first; dims[key_slotentry.second.slot] = key_slotentry.second.dimension; @@ -379,7 +379,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // First build an array of slots gttic(slots); //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. - vector slots(update.size()); + FastVector slots(update.size()); DenseIndex slot = 0; BOOST_FOREACH(Key j, update) { slots[slot] = scatter.at(j).slot; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index c5a995d56..cebf22678 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -19,9 +19,9 @@ #pragma once #include +#include #include -#include #include #include #undef max diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 06fac1a67..4632f538f 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -72,10 +72,10 @@ namespace gtsam { // that is implicitly convertible to T&. This was introduced to work around a problem where // BOOST_FOREACH over terms did not work on GCC. struct _fillTerm { - std::vector& keys; + FastVector& keys; VerticalBlockMatrix& Ab; DenseIndex& i; - _fillTerm(std::vector& keys, VerticalBlockMatrix& Ab, DenseIndex& i) + _fillTerm(FastVector& keys, VerticalBlockMatrix& Ab, DenseIndex& i) : keys(keys), Ab(Ab), i(i) {} template diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index be2636a16..f3b58f2d0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -137,14 +137,14 @@ namespace gtsam { /* ************************************************************************* */ // Helper functions for combine constructor namespace { - boost::tuple, DenseIndex, DenseIndex> _countDims( - const std::vector& factors, const vector& variableSlots) + boost::tuple, DenseIndex, DenseIndex> _countDims( + const FastVector& factors, const FastVector& variableSlots) { gttic(countDims); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - vector varDims(variableSlots.size(), numeric_limits::max()); + FastVector varDims(variableSlots.size(), numeric_limits::max()); #else - vector varDims(variableSlots.size()); + FastVector varDims(variableSlots.size()); #endif DenseIndex m = 0; DenseIndex n = 0; @@ -185,11 +185,11 @@ namespace gtsam { } /* ************************************************************************* */ - std::vector + FastVector _convertOrCastToJacobians(const GaussianFactorGraph& factors) { gttic(Convert_to_Jacobians); - std::vector jacobians; + FastVector jacobians; jacobians.reserve(factors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { if(factor) { @@ -219,13 +219,13 @@ namespace gtsam { } // Cast or convert to Jacobians - std::vector jacobians = _convertOrCastToJacobians(graph); + FastVector jacobians = _convertOrCastToJacobians(graph); gttic(Order_slots); // Order variable slots - we maintain the vector of ordered slots, as well as keep a list // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then // be added after all of the ordered variables. - vector orderedSlots; + FastVector orderedSlots; orderedSlots.reserve(variableSlots->size()); if(ordering) { // If an ordering is provided, arrange the slots first that ordering @@ -259,7 +259,7 @@ namespace gtsam { gttoc(Order_slots); // Count dimensions - vector varDims; + FastVector varDims; DenseIndex m, n; boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots); diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index c7153e349..77e63ba7b 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -128,11 +128,11 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector(const std::vector& keys) const + Vector VectorValues::vector(const FastVector& keys) const { // Count dimensions and collect pointers to avoid double lookups DenseIndex totalDim = 0; - std::vector items(keys.size()); + FastVector items(keys.size()); for(size_t i = 0; i < keys.size(); ++i) { items[i] = &at(keys[i]); totalDim += items[i]->size(); diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 66ab35ca2..125cc8060 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -243,7 +244,7 @@ namespace gtsam { Vector vector() const; /** Access a vector that is a subset of relevant keys. */ - Vector vector(const std::vector& keys) const; + Vector vector(const FastVector& keys) const; /** Swap the data in this VectorValues with another. */ void swap(VectorValues& other); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 087acdee9..8da8bf29e 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -43,7 +43,7 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const std::vector& roots, +void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, @@ -144,7 +144,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const std::vector& roots, +FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { @@ -224,7 +224,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const std::vector& roots, FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const FastVector& roots, FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -272,8 +272,8 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, const Fast if(anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = grad.vector(std::vector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); - Vector gS = grad.vector(std::vector(clique->conditional()->beginParents(), clique->conditional()->endParents())); + Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); + Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); // Compute R*g and S*g for this clique Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 2463f55e7..ee0723232 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -51,7 +51,7 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const std::vector& roots, + static void RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, FastSet& fixedVariables); @@ -79,7 +79,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const std::vector& roots, + static FastSet CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -117,7 +117,7 @@ struct GTSAM_EXPORT ISAM2::Impl { boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - static size_t UpdateDelta(const std::vector& roots, + static size_t UpdateDelta(const FastVector& roots, FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet& replacedKeys, diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index d783eea87..d41100ea2 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -66,7 +66,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, if(recalculate) { // Temporary copy of the original values, to check how much they change - std::vector originalValues(clique->conditional()->nrFrontals()); + FastVector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; @@ -143,7 +143,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh if(recalculate) { // Temporary copy of the original values, to check how much they change - std::vector originalValues(clique->conditional()->nrFrontals()); + FastVector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; @@ -172,7 +172,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh { // Count dimensions of vector DenseIndex dim = 0; - std::vector parentPointers; + FastVector parentPointers; parentPointers.reserve(clique->conditional()->nrParents()); BOOST_FOREACH(Key parent, clique->conditional()->parents()) { parentPointers.push_back(clique->solnPointers_.at(parent)); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 618b5b878..1b3849bb3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -307,7 +307,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(vector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); + this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); gttoc(removetop); // FactorGraph factors(affectedBayesNet); @@ -628,7 +628,7 @@ ISAM2Result ISAM2::update( // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. - vector observedKeys; observedKeys.reserve(markedKeys.size()); + FastVector observedKeys; observedKeys.reserve(markedKeys.size()); BOOST_FOREACH(Index index, markedKeys) { if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them @@ -827,7 +827,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, } // Gather remaining children after we removed marginalized subtrees - vector orphans(clique->children.begin(), clique->children.end()); + FastVector orphans(clique->children.begin(), clique->children.end()); // Add the factors that are pulled into the current clique by the marginalized variables. // These are the factors that involve *marginalized* frontal variables in this clique @@ -852,7 +852,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, FastSet cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); - vector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + FastVector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); pair eliminationResult1 = params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV)); diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp index 4b475a26a..f276c2e63 100644 --- a/gtsam/symbolic/SymbolicFactor.cpp +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -47,7 +48,7 @@ namespace gtsam { const size_t nFrontals = keys.size(); // Build a key vector with the frontals followed by the separator - vector orderedKeys(allKeys.size()); + FastVector orderedKeys(allKeys.size()); std::copy(keys.begin(), keys.end(), orderedKeys.begin()); std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);