12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Using FastVector (with tbb allocator) instead of vector

release/4.3a0
Richard Roberts 2013-08-15 17:21:20 +00:00
parent d3a9c4edcd
commit bd89c5fd45
27 changed files with 116 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/SymmetricBlockMatrix.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -44,7 +45,7 @@ namespace gtsam {
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
std::vector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based) FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.

3
gtsam/base/VerticalBlockMatrix.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -46,7 +47,7 @@ namespace gtsam {
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
std::vector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based) FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment.
DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment.

7
gtsam/base/treeTraversal-inst.h
View File

@ -17,6 +17,7 @@
#pragma once #pragma once
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <stack> #include <stack>
@ -143,7 +144,7 @@ namespace gtsam {
// Set TBB ref count // Set TBB ref count
set_ref_count(1 + (int)roots.size()); set_ref_count(1 + (int)roots.size());
// Create data and tasks for our children // Create data and tasks for our children
std::vector<PreOrderTask*> tasks; FastVector<PreOrderTask*> tasks;
tasks.reserve(roots.size()); tasks.reserve(roots.size());
BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots) BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
{ {
@ -301,12 +302,12 @@ namespace gtsam {
* return a collection of shared pointers to \c FOREST::Node. * return a collection of shared pointers to \c FOREST::Node.
* @return The new collection of roots. */ * @return The new collection of roots. */
template<class FOREST> template<class FOREST>
std::vector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest) FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest)
{ {
typedef typename FOREST::Node Node; typedef typename FOREST::Node Node;
boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>(); boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>); DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
return std::vector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end()); return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end());
} }

10
gtsam/inference/BayesTree-inst.h
View File

@ -340,7 +340,7 @@ namespace gtsam {
// Get the set of variables to eliminate, which is C1\B. // Get the set of variables to eliminate, which is C1\B.
gttic(Full_root_factoring); gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; { boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
std::vector<Index> C1_minus_B; { FastVector<Index> C1_minus_B; {
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) { BOOST_FOREACH(const Index j, *B->conditional()) {
C1_minus_B_set.erase(j); } C1_minus_B_set.erase(j); }
@ -352,7 +352,7 @@ namespace gtsam {
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
} }
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; { boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
std::vector<Index> C2_minus_B; { FastVector<Index> C2_minus_B; {
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) { BOOST_FOREACH(const Index j, *B->conditional()) {
C2_minus_B_set.erase(j); } C2_minus_B_set.erase(j); }
@ -401,12 +401,12 @@ namespace gtsam {
void BayesTree<CLIQUE>::removeClique(sharedClique clique) void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{ {
if (clique->isRoot()) { if (clique->isRoot()) {
typename std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique); typename FastVector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
if(root != roots_.end()) if(root != roots_.end())
roots_.erase(root); roots_.erase(root);
} else { // detach clique from parent } else { // detach clique from parent
sharedClique parent = clique->parent_.lock(); sharedClique parent = clique->parent_.lock();
typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique); typename FastVector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end()); assert(child != parent->children.end());
parent->children.erase(child); parent->children.erase(child);
} }
@ -447,7 +447,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTree<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans) void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
{ {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Key& j, keys) BOOST_FOREACH(const Key& j, keys)

14
gtsam/inference/BayesTree.h
View File

@ -19,7 +19,6 @@
#pragma once #pragma once
#include <vector>
#include <string> #include <string>
#include <tbb/tbb.h> #include <tbb/tbb.h>
#undef max #undef max
@ -29,6 +28,7 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -48,8 +48,8 @@ namespace gtsam {
/** store all the sizes */ /** store all the sizes */
struct GTSAM_EXPORT BayesTreeCliqueData { struct GTSAM_EXPORT BayesTreeCliqueData {
std::vector<std::size_t> conditionalSizes; FastVector<std::size_t> conditionalSizes;
std::vector<std::size_t> separatorSizes; FastVector<std::size_t> separatorSizes;
BayesTreeCliqueStats getStats() const; BayesTreeCliqueStats getStats() const;
}; };
@ -91,7 +91,7 @@ namespace gtsam {
typedef FastList<sharedClique> Cliques; typedef FastList<sharedClique> Cliques;
/** Map from keys to Clique */ /** Map from keys to Clique */
typedef FastMap<Key, sharedClique> Nodes; typedef tbb::concurrent_unordered_map<Key, sharedClique> Nodes;
protected: protected:
@ -99,7 +99,7 @@ namespace gtsam {
Nodes nodes_; Nodes nodes_;
/** Root cliques */ /** Root cliques */
std::vector<sharedClique> roots_; FastVector<sharedClique> roots_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -145,7 +145,7 @@ namespace gtsam {
const sharedNode operator[](Key j) const { return nodes_.at(j); } const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */ /** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; } const FastVector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Index j */ /** alternate syntax for matlab: find the clique that contains the variable with Index j */
const sharedClique& clique(Key j) const { 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. * 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. * Factors and orphans are added to the in/out arguments.
*/ */
void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans); void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
/** /**
* Remove the requested subtree. */ * Remove the requested subtree. */

16
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -41,12 +41,12 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
std::vector<Key> FastVector<Key>
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{ {
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
std::vector<Key> S_setminus_B; FastVector<Key> S_setminus_B;
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B; return S_setminus_B;
@ -54,14 +54,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
std::vector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices( FastVector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
const derived_ptr& B, const FactorGraphType& p_Cp_B) const const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{ {
gttic(shortcut_indices); gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys(); FastSet<Key> allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
std::vector<Key> S_setminus_B = separator_setminus_B(B); FastVector<Key> S_setminus_B = separator_setminus_B(B);
std::vector<Key> keep; FastVector<Key> keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted) // keep = S\B intersect allKeys (S_setminus_B is already sorted)
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep)); allKeys.begin(), allKeys.end(), back_inserter(keep));
@ -114,7 +114,7 @@ namespace gtsam {
gttic(BayesTreeCliqueBase_shortcut); gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B // We only calculate the shortcut when this clique is not B
// and when the S\B is not empty // and when the S\B is not empty
std::vector<Key> S_setminus_B = separator_setminus_B(B); FastVector<Key> S_setminus_B = separator_setminus_B(B);
if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty()) 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 // 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) p_Cp_B += parent->conditional_; // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B // Determine the variables we want to keepSet, S union B
std::vector<Key> keep = shortcut_indices(B, p_Cp_B); FastVector<Key> keep = shortcut_indices(B, p_Cp_B);
// Marginalize out everything except S union B // Marginalize out everything except S union B
boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function); boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
@ -171,7 +171,7 @@ namespace gtsam {
p_Cp += parent->conditional_; // P(Fp|Sp) p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S // The variables we want to keepSet are exactly the ones in S
std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); FastVector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
} }
} }

7
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -19,6 +19,7 @@
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -78,7 +79,7 @@ namespace gtsam {
public: public:
sharedConditional conditional_; sharedConditional conditional_;
derived_weak_ptr parent_; derived_weak_ptr parent_;
std::vector<derived_ptr> children; FastVector<derived_ptr> children;
/// Fill the elimination result produced during elimination. Here this just stores the /// Fill the elimination result produced during elimination. Here this just stores the
/// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
@ -140,12 +141,12 @@ namespace gtsam {
protected: protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
std::vector<Key> separator_setminus_B(const derived_ptr& B) const; FastVector<Key> separator_setminus_B(const derived_ptr& B) const;
/** Determine variable indices to keep in recursive separator shortcut calculation The factor /** 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 * 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. */ * not in S union B. */
std::vector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }

16
gtsam/inference/EliminationTree-inst.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
typename EliminationTree<BAYESNET,GRAPH>::sharedFactor typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
EliminationTree<BAYESNET,GRAPH>::Node::eliminate( EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
const boost::shared_ptr<BayesNetType>& output, const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const const Eliminate& function, const FastVector<sharedFactor>& childrenResults) const
{ {
// This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. // 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()); gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
// Do dense elimination step // Do dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = key; FastVector<Key> keyAsVector(1); keyAsVector[0] = key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, Ordering(keyAsVector)); function(gatheredFactors, Ordering(keyAsVector));
@ -90,10 +90,10 @@ namespace gtsam {
static const size_t none = std::numeric_limits<size_t>::max(); static const size_t none = std::numeric_limits<size_t>::max();
// Allocate result parent vector and vector of last factor columns // Allocate result parent vector and vector of last factor columns
std::vector<sharedNode> nodes(n); FastVector<sharedNode> nodes(n);
std::vector<size_t> parents(n, none); FastVector<size_t> parents(n, none);
std::vector<size_t> prevCol(m, none); FastVector<size_t> prevCol(m, none);
std::vector<bool> factorUsed(m, false); FastVector<bool> factorUsed(m, false);
try { try {
// for column j \in 1 to n do // for column j \in 1 to n do
@ -192,7 +192,7 @@ namespace gtsam {
boost::shared_ptr<BayesNetType> result = boost::make_shared<BayesNetType>(); boost::shared_ptr<BayesNetType> result = boost::make_shared<BayesNetType>();
// Run tree elimination algorithm // Run tree elimination algorithm
std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function); FastVector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
// Add remaining factors that were not involved with eliminated variables // Add remaining factors that were not involved with eliminated variables
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>(); boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
@ -215,7 +215,7 @@ namespace gtsam {
bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
{ {
// Depth-first-traversal stacks // Depth-first-traversal stacks
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2; std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
// Add roots in sorted order // Add roots in sorted order
{ {

15
gtsam/inference/EliminationTree.h
View File

@ -21,6 +21,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree class EliminationTreeTester; // for unit tests, see testEliminationTree
@ -63,15 +64,15 @@ namespace gtsam {
typedef typename GRAPH::Eliminate Eliminate; typedef typename GRAPH::Eliminate Eliminate;
struct Node { struct Node {
typedef std::vector<sharedFactor> Factors; typedef FastVector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children; typedef FastVector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root Key key; ///< key associated with root
Factors factors; ///< factors associated with root Factors factors; ///< factors associated with root
Children children; ///< sub-trees Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output, sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const; const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const; void print(const std::string& str, const KeyFormatter& keyFormatter) const;
}; };
@ -82,8 +83,8 @@ namespace gtsam {
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_; FastVector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_; FastVector<sharedFactor> remainingFactors_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -147,10 +148,10 @@ namespace gtsam {
/// @{ /// @{
/** Return the set of roots (one for a tree, multiple for a forest) */ /** Return the set of roots (one for a tree, multiple for a forest) */
const std::vector<sharedNode>& roots() const { return roots_; } const FastVector<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */ /** Return the remaining factors that are not pulled into elimination */
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; } const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
/** Swap the data of this tree with another one, this operation is very fast. */ /** Swap the data of this tree with another one, this operation is very fast. */
void swap(This& other); void swap(This& other);

12
gtsam/inference/Factor.h
View File

@ -21,10 +21,10 @@
#pragma once #pragma once
#include <vector>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
@ -58,15 +58,15 @@ namespace gtsam {
public: public:
/// Iterator over keys /// Iterator over keys
typedef std::vector<Key>::iterator iterator; typedef FastVector<Key>::iterator iterator;
/// Const iterator over keys /// Const iterator over keys
typedef std::vector<Key>::const_iterator const_iterator; typedef FastVector<Key>::const_iterator const_iterator;
protected: protected:
/// The keys involved in this factor /// The keys involved in this factor
std::vector<Key> keys_; FastVector<Key> keys_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -112,7 +112,7 @@ namespace gtsam {
const_iterator find(Key key) const { return std::find(begin(), end(), key); } const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys /// Access the factor's involved variable keys
const std::vector<Key>& keys() const { return keys_; } const FastVector<Key>& keys() const { return keys_; }
/** Iterator at beginning of involved variable keys */ /** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); } const_iterator begin() const { return keys_.begin(); }
@ -148,7 +148,7 @@ namespace gtsam {
/// @{ /// @{
/** @return keys involved in this factor */ /** @return keys involved in this factor */
std::vector<Key>& keys() { return keys_; } FastVector<Key>& keys() { return keys_; }
/** Iterator at beginning of involved variable keys */ /** Iterator at beginning of involved variable keys */
iterator begin() { return keys_.begin(); } iterator begin() { return keys_.begin(); }

9
gtsam/inference/FactorGraph.h
View File

@ -30,6 +30,7 @@
#include <boost/type_traits.hpp> #include <boost/type_traits.hpp>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
@ -82,8 +83,8 @@ namespace gtsam {
typedef FACTOR FactorType; ///< factor type typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef sharedFactor value_type; typedef sharedFactor value_type;
typedef typename std::vector<sharedFactor>::iterator iterator; typedef typename FastVector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator; typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
private: private:
typedef FactorGraph<FACTOR> This; ///< Typedef for this class typedef FactorGraph<FACTOR> This; ///< Typedef for this class
@ -94,7 +95,7 @@ namespace gtsam {
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */ /** Collection of factors */
std::vector<sharedFactor> factors_; FastVector<sharedFactor> factors_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -142,7 +143,7 @@ namespace gtsam {
/** /**
* Reserve space for the specified number of factors if you know in * 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); } void reserve(size_t size) { factors_.reserve(size); }

6
gtsam/inference/JunctionTree-inst.h
View File

@ -38,8 +38,8 @@ namespace gtsam {
struct ConstructorTraversalData { struct ConstructorTraversalData {
ConstructorTraversalData* const parentData; ConstructorTraversalData* const parentData;
typename JunctionTree<BAYESTREE,GRAPH>::sharedNode myJTNode; typename JunctionTree<BAYESTREE,GRAPH>::sharedNode myJTNode;
std::vector<SymbolicConditional::shared_ptr> childSymbolicConditionals; FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
std::vector<SymbolicFactor::shared_ptr> childSymbolicFactors; FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {}
}; };
@ -128,7 +128,7 @@ namespace gtsam {
struct EliminationData { struct EliminationData {
EliminationData* const parentData; EliminationData* const parentData;
size_t myIndexInParent; size_t myIndexInParent;
std::vector<typename JUNCTIONTREE::sharedFactor> childFactors; FastVector<typename JUNCTIONTREE::sharedFactor> childFactors;
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode; boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode;
EliminationData(EliminationData* _parentData, size_t nChildren) : EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData), parentData(_parentData),

15
gtsam/inference/JunctionTree.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
@ -61,9 +62,9 @@ namespace gtsam {
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
struct Node { struct Node {
typedef std::vector<Key> Keys; typedef FastVector<Key> Keys;
typedef std::vector<sharedFactor> Factors; typedef FastVector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children; typedef FastVector<boost::shared_ptr<Node> > Children;
Keys keys; ///< Frontal keys of this node Keys keys; ///< Frontal keys of this node
Factors factors; ///< Factors associated with this node Factors factors; ///< Factors associated with this node
@ -83,8 +84,8 @@ namespace gtsam {
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_; FastVector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_; FastVector<sharedFactor> remainingFactors_;
protected: protected:
@ -127,10 +128,10 @@ namespace gtsam {
/// @{ /// @{
/** Return the set of roots (one for a tree, multiple for a forest) */ /** Return the set of roots (one for a tree, multiple for a forest) */
const std::vector<sharedNode>& roots() const { return roots_; } const FastVector<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */ /** Return the remaining factors that are not pulled into elimination */
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; } const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
/// @} /// @}

8
gtsam/inference/VariableSlots.h
View File

@ -20,6 +20,7 @@
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <iostream> #include <iostream>
@ -28,7 +29,6 @@
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <string> #include <string>
#include <vector>
namespace gtsam { namespace gtsam {
@ -50,11 +50,11 @@ namespace gtsam {
* *
* \nosubgrouping */ * \nosubgrouping */
class VariableSlots : public FastMap<Index, std::vector<size_t> > { class VariableSlots : public FastMap<Index, FastVector<size_t> > {
public: public:
typedef FastMap<Index, std::vector<size_t> > Base; typedef FastMap<Index, FastVector<size_t> > Base;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -106,7 +106,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
// the array entry for each factor that will indicate the factor // the array entry for each factor that will indicate the factor
// does not involve the variable. // does not involve the variable.
iterator thisVarSlots; bool inserted; iterator thisVarSlots; bool inserted;
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<size_t>())); boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, FastVector<size_t>()));
if(inserted) if(inserted)
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max()); thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot; thisVarSlots->second[jointFactorPos] = factorVarSlot;

5
gtsam/inference/inference-inst.h
View File

@ -24,6 +24,7 @@
#include <utility> #include <utility>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
namespace inference { namespace inference {
@ -33,7 +34,7 @@ namespace gtsam {
template<class TREE> template<class TREE>
struct EliminationData { struct EliminationData {
EliminationData* const parentData; EliminationData* const parentData;
std::vector<typename TREE::sharedFactor> childFactors; FastVector<typename TREE::sharedFactor> childFactors;
EliminationData(EliminationData* _parentData, size_t nChildren) : EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData) { childFactors.reserve(nChildren); } parentData(_parentData) { childFactors.reserve(nChildren); }
}; };
@ -71,7 +72,7 @@ namespace gtsam {
* TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, * TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor,
* TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */ * TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */
template<class TREE, class RESULT> template<class TREE, class RESULT>
std::vector<typename TREE::sharedFactor> FastVector<typename TREE::sharedFactor>
EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function) EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
{ {
// Typedefs // Typedefs

8
gtsam/linear/GaussianConditional.cpp
View File

@ -113,7 +113,7 @@ namespace gtsam {
VectorValues GaussianConditional::solve(const VectorValues& x) const VectorValues GaussianConditional::solve(const VectorValues& x) const
{ {
// Solve matrix // Solve matrix
Vector xS = x.vector(vector<Key>(beginParents(), endParents())); Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
xS = getb() - get_S() * xS; xS = getb() - get_S() * xS;
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
@ -135,8 +135,8 @@ namespace gtsam {
VectorValues GaussianConditional::solveOtherRHS( VectorValues GaussianConditional::solveOtherRHS(
const VectorValues& parents, const VectorValues& rhs) const const VectorValues& parents, const VectorValues& rhs) const
{ {
Vector xS = parents.vector(vector<Key>(beginParents(), endParents())); Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
const Vector rhsR = rhs.vector(vector<Key>(beginFrontals(), endFrontals())); const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS; xS = rhsR - get_S() * xS;
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
@ -158,7 +158,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const
{ {
Vector frontalVec = gy.vector(vector<Key>(beginFrontals(), endFrontals())); Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution // Check for indeterminant solution

4
gtsam/linear/HessianFactor.cpp
View File

@ -282,7 +282,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
gttic(allocate); gttic(allocate);
// Allocate with dimensions for each variable plus 1 at the end for the information vector // Allocate with dimensions for each variable plus 1 at the end for the information vector
keys_.resize(scatter->size()); keys_.resize(scatter->size());
vector<DenseIndex> dims(scatter->size() + 1); FastVector<DenseIndex> dims(scatter->size() + 1);
BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) {
keys_[key_slotentry.second.slot] = key_slotentry.first; keys_[key_slotentry.second.slot] = key_slotentry.first;
dims[key_slotentry.second.slot] = key_slotentry.second.dimension; 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 // First build an array of slots
gttic(slots); gttic(slots);
//size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
vector<DenseIndex> slots(update.size()); FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0; DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) { BOOST_FOREACH(Key j, update) {
slots[slot] = scatter.at(j).slot; slots[slot] = scatter.at(j).slot;

2
gtsam/linear/HessianFactor.h
View File

@ -19,9 +19,9 @@
#pragma once #pragma once
#include <gtsam/base/SymmetricBlockMatrix.h> #include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <vector>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <tbb/tbb.h> #include <tbb/tbb.h>
#undef max #undef max

4
gtsam/linear/JacobianFactor-inl.h
View File

@ -72,10 +72,10 @@ namespace gtsam {
// that is implicitly convertible to T&. This was introduced to work around a problem where // that is implicitly convertible to T&. This was introduced to work around a problem where
// BOOST_FOREACH over terms did not work on GCC. // BOOST_FOREACH over terms did not work on GCC.
struct _fillTerm { struct _fillTerm {
std::vector<Key>& keys; FastVector<Key>& keys;
VerticalBlockMatrix& Ab; VerticalBlockMatrix& Ab;
DenseIndex& i; DenseIndex& i;
_fillTerm(std::vector<Key>& keys, VerticalBlockMatrix& Ab, DenseIndex& i) _fillTerm(FastVector<Key>& keys, VerticalBlockMatrix& Ab, DenseIndex& i)
: keys(keys), Ab(Ab), i(i) {} : keys(keys), Ab(Ab), i(i) {}
template<class MATRIX> template<class MATRIX>

18
gtsam/linear/JacobianFactor.cpp
View File

@ -137,14 +137,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for combine constructor // Helper functions for combine constructor
namespace { namespace {
boost::tuple<vector<DenseIndex>, DenseIndex, DenseIndex> _countDims( boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
const std::vector<JacobianFactor::shared_ptr>& factors, const vector<VariableSlots::const_iterator>& variableSlots) const FastVector<JacobianFactor::shared_ptr>& factors, const FastVector<VariableSlots::const_iterator>& variableSlots)
{ {
gttic(countDims); gttic(countDims);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max()); FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
#else #else
vector<DenseIndex> varDims(variableSlots.size()); FastVector<DenseIndex> varDims(variableSlots.size());
#endif #endif
DenseIndex m = 0; DenseIndex m = 0;
DenseIndex n = 0; DenseIndex n = 0;
@ -185,11 +185,11 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<JacobianFactor::shared_ptr> FastVector<JacobianFactor::shared_ptr>
_convertOrCastToJacobians(const GaussianFactorGraph& factors) _convertOrCastToJacobians(const GaussianFactorGraph& factors)
{ {
gttic(Convert_to_Jacobians); gttic(Convert_to_Jacobians);
std::vector<JacobianFactor::shared_ptr> jacobians; FastVector<JacobianFactor::shared_ptr> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
if(factor) { if(factor) {
@ -219,13 +219,13 @@ namespace gtsam {
} }
// Cast or convert to Jacobians // Cast or convert to Jacobians
std::vector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph); FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
gttic(Order_slots); gttic(Order_slots);
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list // 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 // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
// be added after all of the ordered variables. // be added after all of the ordered variables.
vector<VariableSlots::const_iterator> orderedSlots; FastVector<VariableSlots::const_iterator> orderedSlots;
orderedSlots.reserve(variableSlots->size()); orderedSlots.reserve(variableSlots->size());
if(ordering) { if(ordering) {
// If an ordering is provided, arrange the slots first that ordering // If an ordering is provided, arrange the slots first that ordering
@ -259,7 +259,7 @@ namespace gtsam {
gttoc(Order_slots); gttoc(Order_slots);
// Count dimensions // Count dimensions
vector<DenseIndex> varDims; FastVector<DenseIndex> varDims;
DenseIndex m, n; DenseIndex m, n;
boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots); boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);

4
gtsam/linear/VectorValues.cpp
View File

@ -128,11 +128,11 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector VectorValues::vector(const std::vector<Key>& keys) const Vector VectorValues::vector(const FastVector<Key>& keys) const
{ {
// Count dimensions and collect pointers to avoid double lookups // Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
std::vector<const Vector*> items(keys.size()); FastVector<const Vector*> items(keys.size());
for(size_t i = 0; i < keys.size(); ++i) { for(size_t i = 0; i < keys.size(); ++i) {
items[i] = &at(keys[i]); items[i] = &at(keys[i]);
totalDim += items[i]->size(); totalDim += items[i]->size();

3
gtsam/linear/VectorValues.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
@ -243,7 +244,7 @@ namespace gtsam {
Vector vector() const; Vector vector() const;
/** Access a vector that is a subset of relevant keys. */ /** Access a vector that is a subset of relevant keys. */
Vector vector(const std::vector<Key>& keys) const; Vector vector(const FastVector<Key>& keys) const;
/** Swap the data in this VectorValues with another. */ /** Swap the data in this VectorValues with another. */
void swap(VectorValues& other); void swap(VectorValues& other);

10
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -43,7 +43,7 @@ void ISAM2::Impl::AddVariables(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const std::vector<ISAM2::sharedClique>& roots, void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
FastSet<Key>& replacedKeys, Base::Nodes& nodes, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
@ -144,7 +144,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const std::vector<ISAM2::sharedClique>& roots, FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{ {
@ -224,7 +224,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateDelta(const std::vector<ISAM2::sharedClique>& roots, FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t ISAM2::Impl::UpdateDelta(const FastVector<ISAM2::sharedClique>& roots, FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount; size_t lastBacksubVariableCount;
@ -272,8 +272,8 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const Fast
if(anyReplaced) { if(anyReplaced) {
// Update the current variable // Update the current variable
// Get VectorValues slice corresponding to current variables // Get VectorValues slice corresponding to current variables
Vector gR = grad.vector(std::vector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); Vector gR = grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()));
Vector gS = grad.vector(std::vector<Key>(clique->conditional()->beginParents(), clique->conditional()->endParents())); Vector gS = grad.vector(FastVector<Key>(clique->conditional()->beginParents(), clique->conditional()->endParents()));
// Compute R*g and S*g for this clique // Compute R*g and S*g for this clique
Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS;

6
gtsam/nonlinear/ISAM2-impl.h
View File

@ -51,7 +51,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
/** /**
* Remove variables from the ISAM2 system. * Remove variables from the ISAM2 system.
*/ */
static void RemoveVariables(const FastSet<Key>& unusedKeys, const std::vector<ISAM2::sharedClique>& roots, static void RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes, VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables); FastSet<Key>& fixedVariables);
@ -79,7 +79,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or * @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold * equal to relinearizeThreshold
*/ */
static FastSet<Index> CheckRelinearizationPartial(const std::vector<ISAM2::sharedClique>& roots, static FastSet<Index> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/** /**
@ -117,7 +117,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
boost::optional<VectorValues&> invalidateIfDebug = boost::none, boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter); const KeyFormatter& keyFormatter = DefaultKeyFormatter);
static size_t UpdateDelta(const std::vector<ISAM2::sharedClique>& roots, static size_t UpdateDelta(const FastVector<ISAM2::sharedClique>& roots,
FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold); FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet<Key>& replacedKeys, static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet<Key>& replacedKeys,

6
gtsam/nonlinear/ISAM2-inl.h
View File

@ -66,7 +66,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
if(recalculate) { if(recalculate) {
// Temporary copy of the original values, to check how much they change // Temporary copy of the original values, to check how much they change
std::vector<Vector> originalValues(clique->conditional()->nrFrontals()); FastVector<Vector> originalValues(clique->conditional()->nrFrontals());
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) {
originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; originalValues[it - clique->conditional()->beginFrontals()] = delta[*it];
@ -143,7 +143,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
if(recalculate) if(recalculate)
{ {
// Temporary copy of the original values, to check how much they change // Temporary copy of the original values, to check how much they change
std::vector<Vector> originalValues(clique->conditional()->nrFrontals()); FastVector<Vector> originalValues(clique->conditional()->nrFrontals());
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; originalValues[it - clique->conditional()->beginFrontals()] = delta[*it];
@ -172,7 +172,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
{ {
// Count dimensions of vector // Count dimensions of vector
DenseIndex dim = 0; DenseIndex dim = 0;
std::vector<VectorValues::const_iterator> parentPointers; FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents()); parentPointers.reserve(clique->conditional()->nrParents());
BOOST_FOREACH(Key parent, clique->conditional()->parents()) { BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
parentPointers.push_back(clique->solnPointers_.at(parent)); parentPointers.push_back(clique->solnPointers_.at(parent));

8
gtsam/nonlinear/ISAM2.cpp
View File

@ -307,7 +307,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
gttic(removetop); gttic(removetop);
Cliques orphans; Cliques orphans;
GaussianBayesNet affectedBayesNet; GaussianBayesNet affectedBayesNet;
this->removeTop(vector<Key>(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans);
gttoc(removetop); gttoc(removetop);
// FactorGraph<GaussianFactor> factors(affectedBayesNet); // FactorGraph<GaussianFactor> factors(affectedBayesNet);
@ -628,7 +628,7 @@ ISAM2Result ISAM2::update(
// NOTE: we use assign instead of the iterator constructor here because this // NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to // is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Index value) instead of the iterator constructor. // vector(size_t count, Index value) instead of the iterator constructor.
vector<Index> observedKeys; observedKeys.reserve(markedKeys.size()); FastVector<Index> observedKeys; observedKeys.reserve(markedKeys.size());
BOOST_FOREACH(Index index, markedKeys) { BOOST_FOREACH(Index index, markedKeys) {
if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused 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 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<Key>& leafKeysList,
} }
// Gather remaining children after we removed marginalized subtrees // Gather remaining children after we removed marginalized subtrees
vector<sharedClique> orphans(clique->children.begin(), clique->children.end()); FastVector<sharedClique> orphans(clique->children.begin(), clique->children.end());
// Add the factors that are pulled into the current clique by the marginalized variables. // 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 // These are the factors that involve *marginalized* frontal variables in this clique
@ -852,7 +852,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
FastSet<Index> cliqueFrontalsToEliminate; FastSet<Index> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end()));
vector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); FastVector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> eliminationResult1 = pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> eliminationResult1 =
params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV)); params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV));

3
gtsam/symbolic/SymbolicFactor.cpp
View File

@ -18,6 +18,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/symbolic/SymbolicFactor.h> #include <gtsam/symbolic/SymbolicFactor.h>
#include <gtsam/symbolic/SymbolicConditional.h> #include <gtsam/symbolic/SymbolicConditional.h>
@ -47,7 +48,7 @@ namespace gtsam {
const size_t nFrontals = keys.size(); const size_t nFrontals = keys.size();
// Build a key vector with the frontals followed by the separator // Build a key vector with the frontals followed by the separator
vector<Key> orderedKeys(allKeys.size()); FastVector<Key> orderedKeys(allKeys.size());
std::copy(keys.begin(), keys.end(), orderedKeys.begin()); std::copy(keys.begin(), keys.end(), orderedKeys.begin());
std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals); std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);