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
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
@ -44,7 +45,7 @@ namespace gtsam {
protected:
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.

3
gtsam/base/VerticalBlockMatrix.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
@ -46,7 +47,7 @@ namespace gtsam {
protected:
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 rowEnd_; ///< Changes apparent matrix view, see main class comment.

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

@ -17,6 +17,7 @@
#pragma once
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <stack>
@ -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<PreOrderTask*> tasks;
FastVector<PreOrderTask*> tasks;
tasks.reserve(roots.size());
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 The new collection of roots. */
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;
boost::shared_ptr<Node> rootContainer = boost::make_shared<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.
gttic(Full_root_factoring);
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());
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<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());
BOOST_FOREACH(const Index j, *B->conditional()) {
C2_minus_B_set.erase(j); }
@ -401,12 +401,12 @@ namespace gtsam {
void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{
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())
roots_.erase(root);
} else { // detach clique from parent
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());
parent->children.erase(child);
}
@ -447,7 +447,7 @@ namespace gtsam {
/* ************************************************************************* */
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
BOOST_FOREACH(const Key& j, keys)

14
gtsam/inference/BayesTree.h
View File

@ -19,7 +19,6 @@
#pragma once
#include <vector>
#include <string>
#include <tbb/tbb.h>
#undef max
@ -29,6 +28,7 @@
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
@ -48,8 +48,8 @@ namespace gtsam {
/** store all the sizes */
struct GTSAM_EXPORT BayesTreeCliqueData {
std::vector<std::size_t> conditionalSizes;
std::vector<std::size_t> separatorSizes;
FastVector<std::size_t> conditionalSizes;
FastVector<std::size_t> separatorSizes;
BayesTreeCliqueStats getStats() const;
};
@ -91,7 +91,7 @@ namespace gtsam {
typedef FastList<sharedClique> Cliques;
/** Map from keys to Clique */
typedef FastMap<Key, sharedClique> Nodes;
typedef tbb::concurrent_unordered_map<Key, sharedClique> Nodes;
protected:
@ -99,7 +99,7 @@ namespace gtsam {
Nodes nodes_;
/** Root cliques */
std::vector<sharedClique> roots_;
FastVector<sharedClique> 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<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 */
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<Key>& keys, BayesNetType& bn, Cliques& orphans);
void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
/**
* Remove the requested subtree. */

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

@ -41,12 +41,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
std::vector<Key>
FastVector<Key>
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> 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(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B;
@ -54,14 +54,14 @@ namespace gtsam {
/* ************************************************************************* */
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
{
gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
std::vector<Key> S_setminus_B = separator_setminus_B(B);
std::vector<Key> keep;
FastVector<Key> S_setminus_B = separator_setminus_B(B);
FastVector<Key> 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<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())
{
// 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<Key> keep = shortcut_indices(B, p_Cp_B);
FastVector<Key> keep = shortcut_indices(B, p_Cp_B);
// Marginalize out everything except S union B
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)
// 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);
}
}

7
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -19,6 +19,7 @@
#include <boost/optional.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
@ -78,7 +79,7 @@ namespace gtsam {
public:
sharedConditional conditional_;
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
/// 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<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
* 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<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. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }

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

@ -36,7 +36,7 @@ namespace gtsam {
typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
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.
@ -49,7 +49,7 @@ namespace gtsam {
gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
// 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 =
function(gatheredFactors, Ordering(keyAsVector));
@ -90,10 +90,10 @@ namespace gtsam {
static const size_t none = std::numeric_limits<size_t>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<sharedNode> nodes(n);
std::vector<size_t> parents(n, none);
std::vector<size_t> prevCol(m, none);
std::vector<bool> factorUsed(m, false);
FastVector<sharedNode> nodes(n);
FastVector<size_t> parents(n, none);
FastVector<size_t> prevCol(m, none);
FastVector<bool> factorUsed(m, false);
try {
// for column j \in 1 to n do
@ -192,7 +192,7 @@ namespace gtsam {
boost::shared_ptr<BayesNetType> result = boost::make_shared<BayesNetType>();
// 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
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
{
// Depth-first-traversal stacks
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2;
std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
// Add roots in sorted order
{

15
gtsam/inference/EliminationTree.h
View File

@ -21,6 +21,7 @@
#include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
@ -63,15 +64,15 @@ namespace gtsam {
typedef typename GRAPH::Eliminate Eliminate;
struct Node {
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
Children children; ///< sub-trees
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;
};
@ -82,8 +83,8 @@ namespace gtsam {
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
FastVector<sharedNode> roots_;
FastVector<sharedFactor> 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<sharedNode>& roots() const { return roots_; }
const FastVector<sharedNode>& roots() const { return roots_; }
/** 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. */
void swap(This& other);

12
gtsam/inference/Factor.h
View File

@ -21,10 +21,10 @@
#pragma once
#include <vector>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
@ -58,15 +58,15 @@ namespace gtsam {
public:
/// Iterator over keys
typedef std::vector<Key>::iterator iterator;
typedef FastVector<Key>::iterator iterator;
/// Const iterator over keys
typedef std::vector<Key>::const_iterator const_iterator;
typedef FastVector<Key>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<Key> keys_;
FastVector<Key> 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<Key>& keys() const { return keys_; }
const FastVector<Key>& 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<Key>& keys() { return keys_; }
FastVector<Key>& keys() { return keys_; }
/** Iterator at beginning of involved variable keys */
iterator begin() { return keys_.begin(); }

9
gtsam/inference/FactorGraph.h
View File

@ -30,6 +30,7 @@
#include <boost/type_traits.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
@ -82,8 +83,8 @@ namespace gtsam {
typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef sharedFactor value_type;
typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
typedef typename FastVector<sharedFactor>::iterator iterator;
typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
private:
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
@ -94,7 +95,7 @@ namespace gtsam {
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */
std::vector<sharedFactor> factors_;
FastVector<sharedFactor> 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); }

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

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

15
gtsam/inference/JunctionTree.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
@ -61,9 +62,9 @@ namespace gtsam {
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
struct Node {
typedef std::vector<Key> Keys;
typedef std::vector<sharedFactor> Factors;
typedef std::vector<boost::shared_ptr<Node> > Children;
typedef FastVector<Key> Keys;
typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Node> > 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<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
FastVector<sharedNode> roots_;
FastVector<sharedFactor> remainingFactors_;
protected:
@ -127,10 +128,10 @@ namespace gtsam {
/// @{
/** 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 */
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/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/timing.h>
#include <iostream>
@ -28,7 +29,6 @@
#include <boost/tuple/tuple.hpp>
#include <string>
#include <vector>
namespace gtsam {
@ -50,11 +50,11 @@ namespace gtsam {
*
* \nosubgrouping */
class VariableSlots : public FastMap<Index, std::vector<size_t> > {
class VariableSlots : public FastMap<Index, FastVector<size_t> > {
public:
typedef FastMap<Index, std::vector<size_t> > Base;
typedef FastMap<Index, FastVector<size_t> > 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<size_t>()));
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, FastVector<size_t>()));
if(inserted)
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot;

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

@ -24,6 +24,7 @@
#include <utility>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
namespace inference {
@ -33,7 +34,7 @@ namespace gtsam {
template<class TREE>
struct EliminationData {
EliminationData* const parentData;
std::vector<typename TREE::sharedFactor> childFactors;
FastVector<typename TREE::sharedFactor> 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<class TREE, class RESULT>
std::vector<typename TREE::sharedFactor>
FastVector<typename TREE::sharedFactor>
EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
{
// Typedefs

8
gtsam/linear/GaussianConditional.cpp
View File

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

4
gtsam/linear/HessianFactor.cpp
View File

@ -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<DenseIndex> dims(scatter->size() + 1);
FastVector<DenseIndex> 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<DenseIndex> slots(update.size());
FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) {
slots[slot] = scatter.at(j).slot;

2
gtsam/linear/HessianFactor.h
View File

@ -19,9 +19,9 @@
#pragma once
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/GaussianFactor.h>
#include <vector>
#include <boost/make_shared.hpp>
#include <tbb/tbb.h>
#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
// BOOST_FOREACH over terms did not work on GCC.
struct _fillTerm {
std::vector<Key>& keys;
FastVector<Key>& keys;
VerticalBlockMatrix& Ab;
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) {}
template<class MATRIX>

18
gtsam/linear/JacobianFactor.cpp
View File

@ -137,14 +137,14 @@ namespace gtsam {
/* ************************************************************************* */
// Helper functions for combine constructor
namespace {
boost::tuple<vector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
const std::vector<JacobianFactor::shared_ptr>& factors, const vector<VariableSlots::const_iterator>& variableSlots)
boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
const FastVector<JacobianFactor::shared_ptr>& factors, const FastVector<VariableSlots::const_iterator>& variableSlots)
{
gttic(countDims);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
#else
vector<DenseIndex> varDims(variableSlots.size());
FastVector<DenseIndex> varDims(variableSlots.size());
#endif
DenseIndex m = 0;
DenseIndex n = 0;
@ -185,11 +185,11 @@ namespace gtsam {
}
/* ************************************************************************* */
std::vector<JacobianFactor::shared_ptr>
FastVector<JacobianFactor::shared_ptr>
_convertOrCastToJacobians(const GaussianFactorGraph& factors)
{
gttic(Convert_to_Jacobians);
std::vector<JacobianFactor::shared_ptr> jacobians;
FastVector<JacobianFactor::shared_ptr> 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<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
FastVector<JacobianFactor::shared_ptr> 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<VariableSlots::const_iterator> orderedSlots;
FastVector<VariableSlots::const_iterator> 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<DenseIndex> varDims;
FastVector<DenseIndex> varDims;
DenseIndex m, n;
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
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) {
items[i] = &at(keys[i]);
totalDim += items[i]->size();

3
gtsam/linear/VectorValues.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
#include <boost/shared_ptr.hpp>
@ -243,7 +244,7 @@ namespace gtsam {
Vector vector() const;
/** 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. */
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,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
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 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;
@ -272,8 +272,8 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const Fast
if(anyReplaced) {
// Update the current variable
// Get VectorValues slice corresponding to current variables
Vector gR = grad.vector(std::vector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()));
Vector gS = grad.vector(std::vector<Key>(clique->conditional()->beginParents(), clique->conditional()->endParents()));
Vector gR = grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()));
Vector gS = grad.vector(FastVector<Key>(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;

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

@ -51,7 +51,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
/**
* 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,
VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
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
* 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);
/**
@ -117,7 +117,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
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);
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) {
// 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;
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>& clique, double thresh
if(recalculate)
{
// 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;
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>& clique, double thresh
{
// Count dimensions of vector
DenseIndex dim = 0;
std::vector<VectorValues::const_iterator> parentPointers;
FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents());
BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
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);
Cliques orphans;
GaussianBayesNet affectedBayesNet;
this->removeTop(vector<Key>(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans);
this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans);
gttoc(removetop);
// FactorGraph<GaussianFactor> 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<Index> observedKeys; observedKeys.reserve(markedKeys.size());
FastVector<Index> 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<Key>& leafKeysList,
}
// 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.
// 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;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.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 =
params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV));

3
gtsam/symbolic/SymbolicFactor.cpp
View File

@ -18,6 +18,7 @@
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/symbolic/SymbolicFactor.h>
#include <gtsam/symbolic/SymbolicConditional.h>
@ -47,7 +48,7 @@ namespace gtsam {
const size_t nFrontals = keys.size();
// 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::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);