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

Using FastVector instead of vector in most code called during elimination and solving

release/4.3a0
Richard Roberts 2012-02-10 15:56:01 +00:00
parent 8885b33191
commit 263b50d85a
36 changed files with 173 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/base/FastVector.h
View File

@ -50,14 +50,29 @@ public:
/** Constructor from a range, passes through to base class */ /** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR> template<typename INPUTITERATOR>
explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(first != last)
Base::assign(first, last);
}
/** Copy constructor from another FastSet */ /** Copy constructor from another FastSet */
FastVector(const FastVector<VALUE>& x) : Base(x) {} FastVector(const FastVector<VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */ /** Copy constructor from the base class */
FastVector(const Base& x) : Base(x) {} FastVector(const Base& x) : Base(x) {}
/** Copy constructor from a standard STL container */
FastVector(const std::vector<VALUE>& x) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

2
gtsam/base/Lie.h
View File

@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
template <class T> template <class T>
T expm(const Vector& x, int K=7) { T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(x); Matrix xhat = wedge<T>(x);
return expm(xhat,K); return T(expm(xhat,K));
} }
} // namespace gtsam } // namespace gtsam

4
gtsam/inference/BayesTree.h
View File

@ -297,7 +297,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder( void _BayesTree_dim_adder(
std::vector<size_t>& dims, FastVector<size_t>& dims,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) { const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) { if(clique) {
@ -316,7 +316,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL,class CLIQUE> template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) { boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0); FastVector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root()); _BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions)); return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
} }

6
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -223,7 +223,7 @@ namespace gtsam {
assertInvariants(); assertInvariants();
GenericSequentialSolver<FactorType> solver(p_FSR); GenericSequentialSolver<FactorType> solver(p_FSR);
return *solver.jointFactorGraph(conditional_->keys(), function); return *solver.jointFactorGraph(vector<Index>(conditional_->keys().begin(), conditional_->keys().end()), function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -243,8 +243,8 @@ namespace gtsam {
joint.push_back(R->conditional()->toFactor()); // P(R) joint.push_back(R->conditional()->toFactor()); // P(R)
// Find the keys of both C1 and C2 // Find the keys of both C1 and C2
std::vector<Index> keys1(conditional_->keys()); const FastVector<Index>& keys1(conditional_->keys());
std::vector<Index> keys2(C2->conditional_->keys()); const FastVector<Index>& keys2(C2->conditional_->keys());
FastSet<Index> keys12; FastSet<Index> keys12;
keys12.insert(keys1.begin(), keys1.end()); keys12.insert(keys1.begin(), keys1.end());
keys12.insert(keys2.begin(), keys2.end()); keys12.insert(keys2.begin(), keys2.end());

11
gtsam/inference/ClusterTree.h
View File

@ -20,12 +20,13 @@
#pragma once #pragma once
#include <list> #include <list>
#include <vector>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastList.h>
namespace gtsam { namespace gtsam {
@ -46,13 +47,13 @@ namespace gtsam {
typedef typename boost::shared_ptr<Cluster> shared_ptr; typedef typename boost::shared_ptr<Cluster> shared_ptr;
typedef typename boost::weak_ptr<Cluster> weak_ptr; typedef typename boost::weak_ptr<Cluster> weak_ptr;
const std::vector<Index> frontal; // the frontal variables const FastVector<Index> frontal; // the frontal variables
const std::vector<Index> separator; // the separator variables const FastVector<Index> separator; // the separator variables
protected: protected:
weak_ptr parent_; // the parent cluster weak_ptr parent_; // the parent cluster
std::list<shared_ptr> children_; // the child clusters FastList<shared_ptr> children_; // the child clusters
const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
public: public:
@ -82,7 +83,7 @@ namespace gtsam {
bool equals(const Cluster& other) const; bool equals(const Cluster& other) const;
/// get a reference to the children /// get a reference to the children
const std::list<shared_ptr>& children() const { return children_; } const FastList<shared_ptr>& children() const { return children_; }
/// add a child /// add a child
void addChild(shared_ptr child); void addChild(shared_ptr child);

12
gtsam/inference/Conditional.h
View File

@ -45,8 +45,8 @@ private:
/** Create keys by adding key in front */ /** Create keys by adding key in front */
template<typename ITERATOR> template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { static FastVector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
std::vector<Key> keys((lastParent - firstParent) + 1); FastVector<Key> keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1); std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key; keys[0] = key;
return keys; return keys;
@ -116,8 +116,14 @@ public:
assertInvariants(); assertInvariants();
} }
/** Constructor from a frontal variable and a vector of parents */
Conditional(Key key, const FastVector<Key>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */ /** Constructor from keys and nr of frontal variables */
Conditional(const std::vector<Index>& keys, size_t nrFrontals) : Conditional(const FastVector<Index>& keys, size_t nrFrontals) :
FactorType(keys), nrFrontals_(nrFrontals) { FactorType(keys), nrFrontals_(nrFrontals) {
assertInvariants(); assertInvariants();
} }

10
gtsam/inference/EliminationTree-inl.h
View File

@ -58,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) { FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables // Number of factors and variables
const size_t m = structure.nFactors(); const size_t m = structure.nFactors();
@ -67,8 +67,8 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& struc
static const Index none = numeric_limits<Index>::max(); static const Index none = numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns // Allocate result parent vector and vector of last factor columns
vector<Index> parents(n, none); FastVector<Index> parents(n, none);
vector<Index> prevCol(m, none); FastVector<Index> prevCol(m, none);
// for column j \in 1 to n do // for column j \in 1 to n do
for (Index j = 0; j < n; j++) { for (Index j = 0; j < n; j++) {
@ -100,7 +100,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
tic(1, "ET ComputeParents"); tic(1, "ET ComputeParents");
// Compute the tree structure // Compute the tree structure
vector<Index> parents(ComputeParents(structure)); FastVector<Index> parents(ComputeParents(structure));
toc(1, "ET ComputeParents"); toc(1, "ET ComputeParents");
// Number of variables // Number of variables
@ -110,7 +110,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
// Create tree structure // Create tree structure
tic(2, "assemble tree"); tic(2, "assemble tree");
vector<shared_ptr> trees(n); FastVector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) { for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0 Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationTree(j)); // Create a new node on this variable trees[j].reset(new EliminationTree(j)); // Create a new node on this variable

4
gtsam/inference/EliminationTree.h
View File

@ -54,7 +54,7 @@ private:
typedef FastList<sharedFactor> Factors; typedef FastList<sharedFactor> Factors;
typedef FastList<shared_ptr> SubTrees; typedef FastList<shared_ptr> SubTrees;
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals; typedef FastVector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
Index key_; ///< index associated with root Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root Factors factors_; ///< factors associated with root
@ -74,7 +74,7 @@ private:
* Static internal function to build a vector of parent pointers using the * Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT. * algorithm of Gilbert et al., 2001, BIT.
*/ */
static std::vector<Index> ComputeParents(const VariableIndex& structure); static FastVector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */ /** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); } void add(const sharedFactor& factor) { factors_.push_back(factor); }

21
gtsam/inference/Factor.h
View File

@ -22,10 +22,10 @@
#pragma once #pragma once
#include <set> #include <set>
#include <vector>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -68,15 +68,15 @@ public:
typedef boost::shared_ptr<Factor> shared_ptr; typedef boost::shared_ptr<Factor> shared_ptr;
/// Iterator over keys /// Iterator over keys
typedef typename std::vector<Key>::iterator iterator; typedef typename FastVector<Key>::iterator iterator;
/// Const iterator over keys /// Const iterator over keys
typedef typename std::vector<Key>::const_iterator const_iterator; typedef typename 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_;
friend class JacobianFactor; friend class JacobianFactor;
friend class HessianFactor; friend class HessianFactor;
@ -132,6 +132,11 @@ public:
assertInvariants(); assertInvariants();
} }
/** Construct n-way factor */
Factor(const FastVector<Key>& keys) : keys_(keys) {
assertInvariants();
}
/** Constructor from a collection of keys */ /** Constructor from a collection of keys */
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) : template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); } keys_(beginKey, endKey) { assertInvariants(); }
@ -165,8 +170,8 @@ public:
/// find /// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); } const_iterator find(Key key) const { return std::find(begin(), end(), key); }
///TODO: comment /// @return keys involved in this factor
const std::vector<Key>& keys() const { return keys_; } const FastVector<Key>& keys() const { return keys_; }
/** iterators */ /** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment const_iterator begin() const { return keys_.begin(); } ///TODO: comment
@ -194,7 +199,7 @@ public:
/** /**
* @return keys involved in this factor * @return keys involved in this factor
*/ */
std::vector<Key>& keys() { return keys_; } FastVector<Key>& keys() { return keys_; }
/** mutable iterators */ /** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment iterator begin() { return keys_.begin(); } ///TODO: comment

4
gtsam/inference/FactorGraph-inl.h
View File

@ -104,8 +104,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class KEY> template<class DERIVED, class KEY>
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots) { const FastMap<KEY, FastVector<KEY> >& variableSlots) {
typedef const pair<const KEY, std::vector<KEY> > KeySlotPair; typedef const pair<const KEY, FastVector<KEY> > KeySlotPair;
return typename DERIVED::shared_ptr(new DERIVED( return typename DERIVED::shared_ptr(new DERIVED(
boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)), boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)),
boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1)))); boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1))));

2
gtsam/inference/FactorGraph.h
View File

@ -232,7 +232,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** Create a combined joint factor (new style for EliminationTree). */ /** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVED, class KEY> template<class DERIVED, class KEY>
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots); const FastMap<KEY, FastVector<KEY> >& variableSlots);
/** /**
* static function that combines two factor graphs * static function that combines two factor graphs

11
gtsam/inference/IndexConditional.h
View File

@ -74,12 +74,23 @@ namespace gtsam {
assertInvariants(); assertInvariants();
} }
/** Constructor from a frontal variable and a vector of parents (FastVector version) */
IndexConditional(Index j, const FastVector<Index>& parents) : Base(j, parents) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */ /** Constructor from keys and nr of frontal variables */
IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) : IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) { Base(keys, nrFrontals) {
assertInvariants(); assertInvariants();
} }
/** Constructor from keys and nr of frontal variables (FastVector version) */
IndexConditional(const FastVector<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) {
assertInvariants();
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{

6
gtsam/inference/IndexFactor.h
View File

@ -114,6 +114,12 @@ namespace gtsam {
assertInvariants(); assertInvariants();
} }
/** Construct n-way factor (FastVector version) */
IndexFactor(const FastVector<Index>& js) :
Base(js) {
assertInvariants();
}
/** Constructor from a collection of keys */ /** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, template<class KeyIterator> IndexFactor(KeyIterator beginKey,
KeyIterator endKey) : KeyIterator endKey) :

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

@ -83,7 +83,7 @@ namespace gtsam {
// Two stages - first build an array of the lowest-ordered variable in each // Two stages - first build an array of the lowest-ordered variable in each
// factor and find the last variable to be eliminated. // factor and find the last variable to be eliminated.
vector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max()); FastVector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
Index maxVar = 0; Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i) for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) { if(fg[i]) {
@ -96,7 +96,7 @@ namespace gtsam {
// Now add each factor to the list corresponding to its lowest-ordered // Now add each factor to the list corresponding to its lowest-ordered
// variable. // variable.
vector<FastList<size_t> > targets(maxVar+1); FastVector<FastList<size_t> > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i) for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != numeric_limits<Index>::max()) if(lowestOrdered[i] != numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i); targets[lowestOrdered[i]].push_back(i);
@ -108,7 +108,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG, class BTCLIQUE> template<class FG, class BTCLIQUE>
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg, typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
const std::vector<FastList<size_t> >& targets, const FastVector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) { const SymbolicBayesTree::sharedClique& bayesClique) {
if(bayesClique) { if(bayesClique) {

4
gtsam/inference/JunctionTree.h
View File

@ -20,7 +20,7 @@
#pragma once #pragma once
#include <set> #include <set>
#include <vector> #include <gtsam/base/FastVector.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/ClusterTree.h> #include <gtsam/inference/ClusterTree.h>
@ -78,7 +78,7 @@ namespace gtsam {
const SymbolicBayesTree::sharedClique& clique); const SymbolicBayesTree::sharedClique& clique);
/// distribute the factors along the cluster tree /// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets, sharedClique distributeFactors(const FG& fg, const FastVector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique); const SymbolicBayesTree::sharedClique& clique);
/// recursive elimination function /// recursive elimination function

8
gtsam/inference/Permutation.h
View File

@ -17,12 +17,12 @@
#pragma once #pragma once
#include <vector>
#include <string> #include <string>
#include <iostream> #include <iostream>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -46,12 +46,12 @@ class Inference;
*/ */
class Permutation { class Permutation {
protected: protected:
std::vector<Index> rangeIndices_; FastVector<Index> rangeIndices_;
public: public:
typedef boost::shared_ptr<Permutation> shared_ptr; typedef boost::shared_ptr<Permutation> shared_ptr;
typedef std::vector<Index>::const_iterator const_iterator; typedef FastVector<Index>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator; typedef FastVector<Index>::iterator iterator;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{

6
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -66,7 +66,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
IndexFactor::shared_ptr CombineSymbolic( IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index, const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots) { FastVector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
variableSlots)); variableSlots));
// combined->assertInvariants(); // combined->assertInvariants();
@ -84,9 +84,11 @@ namespace gtsam {
if (keys.size() < 1) throw invalid_argument( if (keys.size() < 1) throw invalid_argument(
"IndexFactor::CombineAndEliminate called on factors with no variables."); "IndexFactor::CombineAndEliminate called on factors with no variables.");
if(nrFrontals > keys.size()) throw invalid_argument(
"Requesting to eliminate more variables than are present in the factors");
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result; pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
std::vector<Index> newKeys(keys.begin(),keys.end()); FastVector<Index> newKeys(keys.begin(),keys.end());
result.first.reset(new IndexConditional(newKeys, nrFrontals)); result.first.reset(new IndexConditional(newKeys, nrFrontals));
result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end())); result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));

2
gtsam/inference/SymbolicFactorGraph.h
View File

@ -84,7 +84,7 @@ namespace gtsam {
/** Create a combined joint factor (new style for EliminationTree). */ /** Create a combined joint factor (new style for EliminationTree). */
IndexFactor::shared_ptr CombineSymbolic( IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index, const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots); FastVector<Index> >& variableSlots);
/** /**
* CombineAndEliminate provides symbolic elimination. * CombineAndEliminate provides symbolic elimination.

6
gtsam/inference/inference.cpp
View File

@ -29,12 +29,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) { Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember) {
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!) // Convert to compressed column major format colamd wants it in (== MATLAB format!)
int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */ int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */ vector<int> A(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ vector<int> p(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
static const bool debug = false; static const bool debug = false;

7
gtsam/inference/inference.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>
@ -48,7 +49,7 @@ namespace gtsam {
/** /**
* Compute a CCOLAMD permutation using the constraint groups in cmember. * Compute a CCOLAMD permutation using the constraint groups in cmember.
*/ */
static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember); static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember);
}; };
@ -56,7 +57,7 @@ namespace gtsam {
template<typename CONSTRAINED> template<typename CONSTRAINED>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
std::vector<int> cmember(variableIndex.size(), 0); FastVector<int> cmember(variableIndex.size(), 0);
// If at least some variables are not constrained to be last, constrain the // If at least some variables are not constrained to be last, constrain the
// ones that should be constrained. // ones that should be constrained.
@ -72,7 +73,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) { inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
std::vector<int> cmember(variableIndex.size(), 0); FastVector<int> cmember(variableIndex.size(), 0);
return PermutationCOLAMD_(variableIndex, cmember); return PermutationCOLAMD_(variableIndex, cmember);
} }

16
gtsam/inference/tests/testJunctionTree.cpp
View File

@ -56,15 +56,15 @@ TEST( JunctionTree, constructor )
SymbolicJunctionTree actual(fg); SymbolicJunctionTree actual(fg);
vector<Index> frontal1; frontal1 += x3, x4; FastVector<Index> frontal1; frontal1 += x3, x4;
vector<Index> frontal2; frontal2 += x2, x1; FastVector<Index> frontal2; frontal2 += x2, x1;
vector<Index> sep1; FastVector<Index> sep1;
vector<Index> sep2; sep2 += x3; FastVector<Index> sep2; sep2 += x3;
CHECK(assert_equal(frontal1, actual.root()->frontal)); CHECK(assert_container_equality(frontal1, actual.root()->frontal));
CHECK(assert_equal(sep1, actual.root()->separator)); CHECK(assert_container_equality(sep1, actual.root()->separator));
LONGS_EQUAL(1, actual.root()->size()); LONGS_EQUAL(1, actual.root()->size());
CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal)); CHECK(assert_container_equality(frontal2, actual.root()->children().front()->frontal));
CHECK(assert_equal(sep2, actual.root()->children().front()->separator)); CHECK(assert_container_equality(sep2, actual.root()->children().front()->separator));
LONGS_EQUAL(2, actual.root()->children().front()->size()); LONGS_EQUAL(2, actual.root()->children().front()->size());
CHECK(assert_equal(*fg[2], *(*actual.root())[0])); CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0])); CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));

2
gtsam/linear/GaussianConditional.h
View File

@ -268,7 +268,7 @@ template<typename ITERATOR, class MATRIX>
GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices, size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) : const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) :
IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_( IndexConditional(FastVector<Index>(firstKey, lastKey), nrFrontals), rsd_(
matrix_), sigmas_(sigmas), permutation_(permutation) { matrix_), sigmas_(sigmas), permutation_(permutation) {
rsd_.assignNoalias(matrices); rsd_.assignNoalias(matrices);
} }

7
gtsam/linear/GaussianFactor.h
View File

@ -74,6 +74,9 @@ namespace gtsam {
/** Construct n-way factor */ /** Construct n-way factor */
GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {} GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
/** Construct n-way factor */
GaussianFactor(const FastVector<Index>& js) : IndexFactor(js) {}
public: public:
typedef GaussianConditional ConditionalType; typedef GaussianConditional ConditionalType;
@ -110,8 +113,8 @@ namespace gtsam {
/** make keys from list, vector, or map of matrices */ /** make keys from list, vector, or map of matrices */
template<typename ITERATOR> template<typename ITERATOR>
static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) { static FastVector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
std::vector<Index> keys; FastVector<Index> keys;
keys.reserve(n); keys.reserve(n);
for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first); for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
return keys; return keys;

14
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -168,11 +168,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for Combine // Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) { static boost::tuple<FastVector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
#ifndef NDEBUG #ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max()); FastVector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else #else
vector<size_t> varDims(variableSlots.size()); FastVector<size_t> varDims(variableSlots.size());
#endif #endif
size_t m = 0; size_t m = 0;
size_t n = 0; size_t n = 0;
@ -221,7 +221,7 @@ break;
if (debug) cout << "Determine dimensions" << endl; if (debug) cout << "Determine dimensions" << endl;
tic(1, "countDims"); tic(1, "countDims");
vector<size_t> varDims; FastVector<size_t> varDims;
size_t m, n; size_t m, n;
boost::tie(varDims, m, n) = countDims(factors, variableSlots); boost::tie(varDims, m, n) = countDims(factors, variableSlots);
if (debug) { if (debug) {
@ -233,7 +233,7 @@ break;
if (debug) cout << "Sort rows" << endl; if (debug) cout << "Sort rows" << endl;
tic(2, "sort rows"); tic(2, "sort rows");
vector<JacobianFactor::_RowSource> rowSources; FastVector<JacobianFactor::_RowSource> rowSources;
rowSources.reserve(m); rowSources.reserve(m);
bool anyConstrained = false; bool anyConstrained = false;
for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) { for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
@ -367,7 +367,7 @@ break;
// Pull out keys and dimensions // Pull out keys and dimensions
tic(2, "keys"); tic(2, "keys");
vector<size_t> dimensions(scatter.size() + 1); FastVector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension; dimensions[var_slot.second.slot] = var_slot.second.dimension;
} }
@ -569,7 +569,7 @@ break;
// Pull out keys and dimensions // Pull out keys and dimensions
tic(2, "keys"); tic(2, "keys");
vector<size_t> dimensions(scatter.size() + 1); FastVector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension; dimensions[var_slot.second.slot] = var_slot.second.dimension;
} }

4
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -21,8 +21,6 @@
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <vector>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
namespace gtsam { namespace gtsam {
@ -68,7 +66,7 @@ namespace gtsam {
// Allocate solution vector and copy RHS // Allocate solution vector and copy RHS
tic(2, "allocate VectorValues"); tic(2, "allocate VectorValues");
vector<size_t> dims(rootClique->conditional()->back()+1, 0); FastVector<size_t> dims(rootClique->conditional()->back()+1, 0);
countDims(rootClique, dims); countDims(rootClique, dims);
VectorValues result(dims); VectorValues result(dims);
btreeRHS(rootClique, result); btreeRHS(rootClique, result);

4
gtsam/linear/HessianFactor.cpp
View File

@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors, HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) : const FastVector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) { info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL"); const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
@ -505,7 +505,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
tic(2, "remaining factor"); tic(2, "remaining factor");
info_.blockStart() = nrFrontals; info_.blockStart() = nrFrontals;
// Assign the keys // Assign the keys
vector<Index> remainingKeys(keys_.size() - nrFrontals); FastVector<Index> remainingKeys(keys_.size() - nrFrontals);
remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
keys_.swap(remainingKeys); keys_.swap(remainingKeys);
toc(2, "remaining factor"); toc(2, "remaining factor");

2
gtsam/linear/HessianFactor.h
View File

@ -204,7 +204,7 @@ namespace gtsam {
/** Special constructor used in EliminateCholesky which combines the given factors */ /** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors, HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter); const FastVector<size_t>& dimensions, const Scatter& scatter);
/** Destructor */ /** Destructor */
virtual ~HessianFactor() {} virtual ~HessianFactor() {}

12
gtsam/linear/JacobianFactor.cpp
View File

@ -113,7 +113,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms, JacobianFactor::JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) : const Vector &b, const SharedDiagonal& model) :
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
@ -260,7 +260,7 @@ namespace gtsam {
sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j)); sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
// Build a vector of variable dimensions in the new order // Build a vector of variable dimensions in the new order
vector<size_t> dimensions(size() + 1); FastVector<size_t> dimensions(size() + 1);
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
dimensions[j++] = Ab_(sourceSlot.second).cols(); dimensions[j++] = Ab_(sourceSlot.second).cols();
@ -269,7 +269,7 @@ namespace gtsam {
dimensions.back() = 1; dimensions.back() = 1;
// Copy the variables and matrix into the new order // Copy the variables and matrix into the new order
vector<Index> oldKeys(size()); FastVector<Index> oldKeys(size());
keys_.swap(oldKeys); keys_.swap(oldKeys);
AbMatrix oldMatrix; AbMatrix oldMatrix;
BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows()); BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
@ -506,7 +506,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const { void JacobianFactor::collectInfo(size_t index, FastVector<_RowSource>& rowSources) const {
assertInvariants(); assertInvariants();
for (size_t row = 0; row < rows(); ++row) { for (size_t row = 0; row < rows(); ++row) {
Index firstNonzeroVar; Index firstNonzeroVar;
@ -522,7 +522,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::allocate(const VariableSlots& variableSlots, vector< void JacobianFactor::allocate(const VariableSlots& variableSlots, FastVector<
size_t>& varDims, size_t m) { size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size()); keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), begin(), std::transform(variableSlots.begin(), variableSlots.end(), begin(),
@ -551,7 +551,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::copyFNZ(size_t m, size_t n, void JacobianFactor::copyFNZ(size_t m, size_t n,
vector<_RowSource>& rowSources) { FastVector<_RowSource>& rowSources) {
Index i = 0; Index i = 0;
for (size_t row = 0; row < m; ++row) { for (size_t row = 0; row < m; ++row) {
while (i < n && rowSources[row].firstNonzeroVar > keys_[i]) while (i < n && rowSources[row].firstNonzeroVar > keys_[i])

10
gtsam/linear/JacobianFactor.h
View File

@ -85,7 +85,7 @@ namespace gtsam {
protected: protected:
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
std::vector<size_t> firstNonzeroBlocks_; FastVector<size_t> firstNonzeroBlocks_;
AbMatrix matrix_; // the full matrix corresponding to the factor AbMatrix matrix_; // the full matrix corresponding to the factor
BlockAb Ab_; // the block view of the full matrix BlockAb Ab_; // the block view of the full matrix
typedef GaussianFactor Base; // typedef to base typedef GaussianFactor Base; // typedef to base
@ -123,7 +123,7 @@ namespace gtsam {
const Vector& b, const SharedDiagonal& model); const Vector& b, const SharedDiagonal& model);
/** Construct an n-ary factor */ /** Construct an n-ary factor */
JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms, JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model); const Vector &b, const SharedDiagonal& model);
JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms, JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms,
@ -268,18 +268,18 @@ namespace gtsam {
// Many imperative, perhaps all need to be combined in constructor // Many imperative, perhaps all need to be combined in constructor
/** Collect information on Jacobian rows */ /** Collect information on Jacobian rows */
void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const; void collectInfo(size_t index, FastVector<_RowSource>& rowSources) const;
/** allocate space */ /** allocate space */
void allocate(const VariableSlots& variableSlots, void allocate(const VariableSlots& variableSlots,
std::vector<size_t>& varDims, size_t m); FastVector<size_t>& varDims, size_t m);
/** copy a slot from source */ /** copy a slot from source */
void copyRow(const JacobianFactor& source, void copyRow(const JacobianFactor& source,
Index sourceRow, size_t sourceSlot, size_t row, Index slot); Index sourceRow, size_t sourceSlot, size_t row, Index slot);
/** copy firstNonzeroBlocks structure */ /** copy firstNonzeroBlocks structure */
void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources); void copyFNZ(size_t m, size_t n, FastVector<_RowSource>& rowSources);
/** set noiseModel correctly */ /** set noiseModel correctly */
void setModel(bool anyConstrained, const Vector& sigmas); void setModel(bool anyConstrained, const Vector& sigmas);

6
gtsam/linear/NoiseModel.cpp
View File

@ -163,7 +163,7 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
return Unit::Create(maxrank); return Unit::Create(maxrank);
} }
void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const { void Gaussian::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); } BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
whitenInPlace(b); whitenInPlace(b);
} }
@ -513,7 +513,7 @@ Vector Base::sqrtWeight(const Vector &error) const {
* according to their weight implementation */ * according to their weight implementation */
/** Reweight n block matrices with one error vector */ /** Reweight n block matrices with one error vector */
void Base::reweight(vector<Matrix> &A, Vector &error) const { void Base::reweight(FastVector<Matrix> &A, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
BOOST_FOREACH(Matrix& Aj, A) { BOOST_FOREACH(Matrix& Aj, A) {
@ -662,7 +662,7 @@ bool Robust::equals(const Base& expected, double tol) const {
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
} }
void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const { void Robust::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
noise_->WhitenSystem(A,b); noise_->WhitenSystem(A,b);
robust_->reweight(A,b); robust_->reweight(A,b);
} }

9
gtsam/linear/NoiseModel.h
View File

@ -20,6 +20,7 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
@ -76,7 +77,7 @@ namespace gtsam {
virtual double distance(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0;
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0; virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
@ -185,7 +186,7 @@ namespace gtsam {
/** /**
* Whiten a system, in place as well * Whiten a system, in place as well
*/ */
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ; virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A, Vector& b) const ; virtual void WhitenSystem(Matrix& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
@ -619,7 +620,7 @@ namespace gtsam {
Vector sqrtWeight(const Vector &error) const; Vector sqrtWeight(const Vector &error) const;
/** reweight block matrices and a vector according to their weight implementation */ /** reweight block matrices and a vector according to their weight implementation */
void reweight(std::vector<Matrix> &A, Vector &error) const; void reweight(FastVector<Matrix> &A, Vector &error) const;
void reweight(Matrix &A, Vector &error) const; void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
@ -714,7 +715,7 @@ namespace gtsam {
// TODO: these are really robust iterated re-weighting support functions // TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const; virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A, Vector& b) const; virtual void WhitenSystem(Matrix& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;

2
gtsam/nonlinear/GaussianISAM2-inl.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
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
vector<Vector> originalValues((*clique)->nrFrontals()); FastVector<Vector> originalValues((*clique)->nrFrontals());
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
originalValues[it - (*clique)->beginFrontals()] = delta[*it]; originalValues[it - (*clique)->beginFrontals()] = delta[*it];

4
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -126,7 +126,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
theta.insert(newTheta); theta.insert(newTheta);
if(debug) newTheta.print("The new variables are: "); if(debug) newTheta.print("The new variables are: ");
// Add the new keys onto the ordering, add zeros to the delta for the new variables // Add the new keys onto the ordering, add zeros to the delta for the new variables
std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary())); FastVector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
const size_t newDim = accumulate(dims.begin(), dims.end(), 0); const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
const size_t originalDim = delta->dim(); const size_t originalDim = delta->dim();
@ -287,7 +287,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
toc(2,"variable index"); toc(2,"variable index");
tic(3,"ccolamd"); tic(3,"ccolamd");
vector<int> cmember(affectedKeysSelector.size(), 0); FastVector<int> cmember(affectedKeysSelector.size(), 0);
if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) { if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
assert(reorderingMode.constrainedKeys); assert(reorderingMode.constrainedKeys);
if(keys.size() > reorderingMode.constrainedKeys->size()) { if(keys.size() > reorderingMode.constrainedKeys->size()) {

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

@ -229,7 +229,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
tic(1,"reorder"); tic(1,"reorder");
tic(1,"CCOLAMD"); tic(1,"CCOLAMD");
// Do a batch step - reorder and relinearize all variables // Do a batch step - reorder and relinearize all variables
vector<int> cmember(theta_.size(), 0); FastVector<int> cmember(theta_.size(), 0);
FastSet<Index> constrainedKeysSet; FastSet<Index> constrainedKeysSet;
if(constrainKeys) if(constrainKeys)
constrainedKeysSet = *constrainKeys; constrainedKeysSet = *constrainKeys;

34
gtsam/nonlinear/NonlinearFactor.h
View File

@ -24,7 +24,6 @@
#include <limits> #include <limits>
#include <boost/serialization/base_object.hpp> #include <boost/serialization/base_object.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>
@ -36,19 +35,6 @@
namespace gtsam { namespace gtsam {
using boost::make_tuple;
// Helper function to fill a vector from a tuple function of any length
template<typename CONS>
inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
vector[position] = tuple.get_head();
__fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
}
template<>
inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
// Do nothing
}
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Nonlinear factor base class * Nonlinear factor base class
@ -139,7 +125,7 @@ public:
* variable indices. * variable indices.
*/ */
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
std::vector<Index> indices(this->size()); FastVector<Index> indices(this->size());
for(size_t j=0; j<this->size(); ++j) for(size_t j=0; j<this->size(); ++j)
indices[j] = ordering[this->keys()[j]]; indices[j] = ordering[this->keys()[j]];
return IndexFactor::shared_ptr(new IndexFactor(indices)); return IndexFactor::shared_ptr(new IndexFactor(indices));
@ -228,7 +214,7 @@ public:
* If any of the optional Matrix reference arguments are specified, it should compute * If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/ */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0; virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const = 0;
/** /**
* Vector of errors, whitened * Vector of errors, whitened
@ -264,12 +250,12 @@ public:
// Create the set of terms - Jacobians for each index // Create the set of terms - Jacobians for each index
Vector b; Vector b;
// Call evaluate error to get Jacobians and b vector // Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size()); FastVector<Matrix> A(this->size());
b = -unwhitenedError(x, A); b = -unwhitenedError(x, A);
this->noiseModel_->WhitenSystem(A,b); this->noiseModel_->WhitenSystem(A,b);
std::vector<std::pair<Index, Matrix> > terms(this->size()); FastVector<std::pair<Index, Matrix> > terms(this->size());
// Fill in terms // Fill in terms
for(size_t j=0; j<this->size(); ++j) { for(size_t j=0; j<this->size(); ++j) {
terms[j].first = ordering[this->keys()[j]]; terms[j].first = ordering[this->keys()[j]];
@ -339,7 +325,7 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X& x1 = x.at<X>(keys_[0]); const X& x1 = x.at<X>(keys_[0]);
if(H) { if(H) {
@ -422,7 +408,7 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X1& x1 = x.at<X1>(keys_[0]); const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]); const X2& x2 = x.at<X2>(keys_[1]);
@ -512,7 +498,7 @@ public:
/** Calls the 3-key specific version of evaluateError, which is pure virtual /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@ -607,7 +593,7 @@ public:
/** Calls the 4-key specific version of evaluateError, which is pure virtual /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@ -707,7 +693,7 @@ public:
/** Calls the 5-key specific version of evaluateError, which is pure virtual /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@ -813,7 +799,7 @@ public:
/** Calls the 6-key specific version of evaluateError, which is pure virtual /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

32
tests/testGaussianJunctionTree.cpp
View File

@ -59,29 +59,29 @@ TEST( GaussianJunctionTree, constructor2 )
// create an ordering // create an ordering
GaussianJunctionTree actual(fg); GaussianJunctionTree actual(fg);
vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"]; FastVector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"]; FastVector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
vector<Index> frontal3; frontal3 += ordering["x1"]; FastVector<Index> frontal3; frontal3 += ordering["x1"];
vector<Index> frontal4; frontal4 += ordering["x7"]; FastVector<Index> frontal4; frontal4 += ordering["x7"];
vector<Index> sep1; FastVector<Index> sep1;
vector<Index> sep2; sep2 += ordering["x4"]; FastVector<Index> sep2; sep2 += ordering["x4"];
vector<Index> sep3; sep3 += ordering["x2"]; FastVector<Index> sep3; sep3 += ordering["x2"];
vector<Index> sep4; sep4 += ordering["x6"]; FastVector<Index> sep4; sep4 += ordering["x6"];
EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_container_equality(frontal1, actual.root()->frontal));
EXPECT(assert_equal(sep1, actual.root()->separator)); EXPECT(assert_container_equality(sep1, actual.root()->separator));
LONGS_EQUAL(5, actual.root()->size()); LONGS_EQUAL(5, actual.root()->size());
list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin(); list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it; list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTree::sharedClique child0 = *child0it; GaussianJunctionTree::sharedClique child0 = *child0it;
GaussianJunctionTree::sharedClique child1 = *child1it; GaussianJunctionTree::sharedClique child1 = *child1it;
EXPECT(assert_equal(frontal2, child0->frontal)); EXPECT(assert_container_equality(frontal2, child0->frontal));
EXPECT(assert_equal(sep2, child0->separator)); EXPECT(assert_container_equality(sep2, child0->separator));
LONGS_EQUAL(4, child0->size()); LONGS_EQUAL(4, child0->size());
EXPECT(assert_equal(frontal3, child0->children().front()->frontal)); EXPECT(assert_container_equality(frontal3, child0->children().front()->frontal));
EXPECT(assert_equal(sep3, child0->children().front()->separator)); EXPECT(assert_container_equality(sep3, child0->children().front()->separator));
LONGS_EQUAL(2, child0->children().front()->size()); LONGS_EQUAL(2, child0->children().front()->size());
EXPECT(assert_equal(frontal4, child1->frontal)); EXPECT(assert_container_equality(frontal4, child1->frontal));
EXPECT(assert_equal(sep4, child1->separator)); EXPECT(assert_container_equality(sep4, child1->separator));
LONGS_EQUAL(2, child1->size()); LONGS_EQUAL(2, child1->size());
} }