Using FastVector instead of vector in most code called during elimination and solving
parent
8885b33191
commit
263b50d85a
|
@ -50,14 +50,29 @@ public:
|
|||
|
||||
/** Constructor from a range, passes through to base class */
|
||||
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 */
|
||||
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) {}
|
||||
|
||||
/** 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:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
|
|||
template <class T>
|
||||
T expm(const Vector& x, int K=7) {
|
||||
Matrix xhat = wedge<T>(x);
|
||||
return expm(xhat,K);
|
||||
return T(expm(xhat,K));
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
@ -297,7 +297,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void _BayesTree_dim_adder(
|
||||
std::vector<size_t>& dims,
|
||||
FastVector<size_t>& dims,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
|
@ -316,7 +316,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL,class CLIQUE>
|
||||
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());
|
||||
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
|
||||
}
|
||||
|
|
|
@ -223,7 +223,7 @@ namespace gtsam {
|
|||
|
||||
assertInvariants();
|
||||
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)
|
||||
|
||||
// Find the keys of both C1 and C2
|
||||
std::vector<Index> keys1(conditional_->keys());
|
||||
std::vector<Index> keys2(C2->conditional_->keys());
|
||||
const FastVector<Index>& keys1(conditional_->keys());
|
||||
const FastVector<Index>& keys2(C2->conditional_->keys());
|
||||
FastSet<Index> keys12;
|
||||
keys12.insert(keys1.begin(), keys1.end());
|
||||
keys12.insert(keys2.begin(), keys2.end());
|
||||
|
|
|
@ -20,12 +20,13 @@
|
|||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -46,13 +47,13 @@ namespace gtsam {
|
|||
typedef typename boost::shared_ptr<Cluster> shared_ptr;
|
||||
typedef typename boost::weak_ptr<Cluster> weak_ptr;
|
||||
|
||||
const std::vector<Index> frontal; // the frontal variables
|
||||
const std::vector<Index> separator; // the separator variables
|
||||
const FastVector<Index> frontal; // the frontal variables
|
||||
const FastVector<Index> separator; // the separator variables
|
||||
|
||||
protected:
|
||||
|
||||
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
|
||||
|
||||
public:
|
||||
|
@ -82,7 +83,7 @@ namespace gtsam {
|
|||
bool equals(const Cluster& other) const;
|
||||
|
||||
/// 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
|
||||
void addChild(shared_ptr child);
|
||||
|
|
|
@ -45,8 +45,8 @@ private:
|
|||
|
||||
/** Create keys by adding key in front */
|
||||
template<typename ITERATOR>
|
||||
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
|
||||
std::vector<Key> keys((lastParent - firstParent) + 1);
|
||||
static FastVector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
|
||||
FastVector<Key> keys((lastParent - firstParent) + 1);
|
||||
std::copy(firstParent, lastParent, keys.begin() + 1);
|
||||
keys[0] = key;
|
||||
return keys;
|
||||
|
@ -116,8 +116,14 @@ public:
|
|||
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 */
|
||||
Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
||||
Conditional(const FastVector<Index>& keys, size_t nrFrontals) :
|
||||
FactorType(keys), nrFrontals_(nrFrontals) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
|
|
@ -58,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
|
||||
FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
|
||||
|
||||
// Number of factors and variables
|
||||
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();
|
||||
|
||||
// Allocate result parent vector and vector of last factor columns
|
||||
vector<Index> parents(n, none);
|
||||
vector<Index> prevCol(m, none);
|
||||
FastVector<Index> parents(n, none);
|
||||
FastVector<Index> prevCol(m, none);
|
||||
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
|
@ -100,7 +100,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
|
|||
|
||||
tic(1, "ET ComputeParents");
|
||||
// Compute the tree structure
|
||||
vector<Index> parents(ComputeParents(structure));
|
||||
FastVector<Index> parents(ComputeParents(structure));
|
||||
toc(1, "ET ComputeParents");
|
||||
|
||||
// Number of variables
|
||||
|
@ -110,7 +110,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
|
|||
|
||||
// Create tree structure
|
||||
tic(2, "assemble tree");
|
||||
vector<shared_ptr> trees(n);
|
||||
FastVector<shared_ptr> trees(n);
|
||||
for (Index k = 1; k <= n; k++) {
|
||||
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
|
||||
|
|
|
@ -54,7 +54,7 @@ private:
|
|||
|
||||
typedef FastList<sharedFactor> Factors;
|
||||
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
|
||||
Factors factors_; ///< factors associated with root
|
||||
|
@ -74,7 +74,7 @@ private:
|
|||
* Static internal function to build a vector of parent pointers using the
|
||||
* 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 */
|
||||
void add(const sharedFactor& factor) { factors_.push_back(factor); }
|
||||
|
|
|
@ -22,10 +22,10 @@
|
|||
#pragma once
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
#include <gtsam/base/FastVector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -68,15 +68,15 @@ public:
|
|||
typedef boost::shared_ptr<Factor> shared_ptr;
|
||||
|
||||
/// Iterator over keys
|
||||
typedef typename std::vector<Key>::iterator iterator;
|
||||
typedef typename FastVector<Key>::iterator iterator;
|
||||
|
||||
/// Const iterator over keys
|
||||
typedef typename std::vector<Key>::const_iterator const_iterator;
|
||||
typedef typename FastVector<Key>::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
|
||||
/// The keys involved in this factor
|
||||
std::vector<Key> keys_;
|
||||
FastVector<Key> keys_;
|
||||
|
||||
friend class JacobianFactor;
|
||||
friend class HessianFactor;
|
||||
|
@ -132,6 +132,11 @@ public:
|
|||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct n-way factor */
|
||||
Factor(const FastVector<Key>& keys) : keys_(keys) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
|
||||
keys_(beginKey, endKey) { assertInvariants(); }
|
||||
|
@ -165,8 +170,8 @@ public:
|
|||
/// find
|
||||
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
|
||||
|
||||
///TODO: comment
|
||||
const std::vector<Key>& keys() const { return keys_; }
|
||||
/// @return keys involved in this factor
|
||||
const FastVector<Key>& keys() const { return keys_; }
|
||||
|
||||
/** iterators */
|
||||
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
|
||||
|
@ -194,7 +199,7 @@ public:
|
|||
/**
|
||||
* @return keys involved in this factor
|
||||
*/
|
||||
std::vector<Key>& keys() { return keys_; }
|
||||
FastVector<Key>& keys() { return keys_; }
|
||||
|
||||
/** mutable iterators */
|
||||
iterator begin() { return keys_.begin(); } ///TODO: comment
|
||||
|
|
|
@ -104,8 +104,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class DERIVED, class KEY>
|
||||
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
|
||||
const FastMap<KEY, std::vector<KEY> >& variableSlots) {
|
||||
typedef const pair<const KEY, std::vector<KEY> > KeySlotPair;
|
||||
const FastMap<KEY, FastVector<KEY> >& variableSlots) {
|
||||
typedef const pair<const KEY, FastVector<KEY> > KeySlotPair;
|
||||
return typename DERIVED::shared_ptr(new DERIVED(
|
||||
boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)),
|
||||
boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1))));
|
||||
|
|
|
@ -232,7 +232,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
/** Create a combined joint factor (new style for EliminationTree). */
|
||||
template<class DERIVED, class KEY>
|
||||
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
|
||||
|
|
|
@ -74,12 +74,23 @@ namespace gtsam {
|
|||
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 */
|
||||
IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
||||
Base(keys, nrFrontals) {
|
||||
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
|
||||
/// @{
|
||||
|
|
|
@ -114,6 +114,12 @@ namespace gtsam {
|
|||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct n-way factor (FastVector version) */
|
||||
IndexFactor(const FastVector<Index>& js) :
|
||||
Base(js) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
|
||||
KeyIterator endKey) :
|
||||
|
|
|
@ -83,7 +83,7 @@ namespace gtsam {
|
|||
|
||||
// Two stages - first build an array of the lowest-ordered variable in each
|
||||
// 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;
|
||||
for(size_t i=0; i<fg.size(); ++i)
|
||||
if(fg[i]) {
|
||||
|
@ -96,7 +96,7 @@ namespace gtsam {
|
|||
|
||||
// Now add each factor to the list corresponding to its lowest-ordered
|
||||
// variable.
|
||||
vector<FastList<size_t> > targets(maxVar+1);
|
||||
FastVector<FastList<size_t> > targets(maxVar+1);
|
||||
for(size_t i=0; i<lowestOrdered.size(); ++i)
|
||||
if(lowestOrdered[i] != numeric_limits<Index>::max())
|
||||
targets[lowestOrdered[i]].push_back(i);
|
||||
|
@ -108,7 +108,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
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) {
|
||||
|
||||
if(bayesClique) {
|
||||
|
|
|
@ -20,7 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
|
@ -78,7 +78,7 @@ namespace gtsam {
|
|||
const SymbolicBayesTree::sharedClique& clique);
|
||||
|
||||
/// 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);
|
||||
|
||||
/// recursive elimination function
|
||||
|
|
|
@ -17,12 +17,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -46,12 +46,12 @@ class Inference;
|
|||
*/
|
||||
class Permutation {
|
||||
protected:
|
||||
std::vector<Index> rangeIndices_;
|
||||
FastVector<Index> rangeIndices_;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<Permutation> shared_ptr;
|
||||
typedef std::vector<Index>::const_iterator const_iterator;
|
||||
typedef std::vector<Index>::iterator iterator;
|
||||
typedef FastVector<Index>::const_iterator const_iterator;
|
||||
typedef FastVector<Index>::iterator iterator;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
|
@ -66,7 +66,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
IndexFactor::shared_ptr CombineSymbolic(
|
||||
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
|
||||
std::vector<Index> >& variableSlots) {
|
||||
FastVector<Index> >& variableSlots) {
|
||||
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
|
||||
variableSlots));
|
||||
// combined->assertInvariants();
|
||||
|
@ -84,9 +84,11 @@ namespace gtsam {
|
|||
|
||||
if (keys.size() < 1) throw invalid_argument(
|
||||
"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;
|
||||
std::vector<Index> newKeys(keys.begin(),keys.end());
|
||||
FastVector<Index> newKeys(keys.begin(),keys.end());
|
||||
result.first.reset(new IndexConditional(newKeys, nrFrontals));
|
||||
result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
|
||||
|
||||
|
|
|
@ -84,7 +84,7 @@ namespace gtsam {
|
|||
/** Create a combined joint factor (new style for EliminationTree). */
|
||||
IndexFactor::shared_ptr CombineSymbolic(
|
||||
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
|
||||
std::vector<Index> >& variableSlots);
|
||||
FastVector<Index> >& variableSlots);
|
||||
|
||||
/**
|
||||
* CombineAndEliminate provides symbolic elimination.
|
||||
|
|
|
@ -29,12 +29,12 @@ using namespace std;
|
|||
|
||||
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();
|
||||
// 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 */
|
||||
vector<int> A = vector<int>(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> A(Alen); /* colamd arg 4: row indices of A, of size Alen */
|
||||
vector<int> p(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
|
@ -48,7 +49,7 @@ namespace gtsam {
|
|||
/**
|
||||
* 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>
|
||||
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
|
||||
// ones that should be constrained.
|
||||
|
@ -72,7 +73,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
@ -56,15 +56,15 @@ TEST( JunctionTree, constructor )
|
|||
|
||||
SymbolicJunctionTree actual(fg);
|
||||
|
||||
vector<Index> frontal1; frontal1 += x3, x4;
|
||||
vector<Index> frontal2; frontal2 += x2, x1;
|
||||
vector<Index> sep1;
|
||||
vector<Index> sep2; sep2 += x3;
|
||||
CHECK(assert_equal(frontal1, actual.root()->frontal));
|
||||
CHECK(assert_equal(sep1, actual.root()->separator));
|
||||
FastVector<Index> frontal1; frontal1 += x3, x4;
|
||||
FastVector<Index> frontal2; frontal2 += x2, x1;
|
||||
FastVector<Index> sep1;
|
||||
FastVector<Index> sep2; sep2 += x3;
|
||||
CHECK(assert_container_equality(frontal1, actual.root()->frontal));
|
||||
CHECK(assert_container_equality(sep1, actual.root()->separator));
|
||||
LONGS_EQUAL(1, actual.root()->size());
|
||||
CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
|
||||
CHECK(assert_equal(sep2, actual.root()->children().front()->separator));
|
||||
CHECK(assert_container_equality(frontal2, actual.root()->children().front()->frontal));
|
||||
CHECK(assert_container_equality(sep2, actual.root()->children().front()->separator));
|
||||
LONGS_EQUAL(2, actual.root()->children().front()->size());
|
||||
CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
|
||||
CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
|
||||
|
|
|
@ -268,7 +268,7 @@ template<typename ITERATOR, class MATRIX>
|
|||
GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
|
||||
size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
|
||||
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) {
|
||||
rsd_.assignNoalias(matrices);
|
||||
}
|
||||
|
|
|
@ -74,6 +74,9 @@ namespace gtsam {
|
|||
/** Construct n-way factor */
|
||||
GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
|
||||
|
||||
/** Construct n-way factor */
|
||||
GaussianFactor(const FastVector<Index>& js) : IndexFactor(js) {}
|
||||
|
||||
public:
|
||||
|
||||
typedef GaussianConditional ConditionalType;
|
||||
|
@ -110,8 +113,8 @@ namespace gtsam {
|
|||
|
||||
/** make keys from list, vector, or map of matrices */
|
||||
template<typename ITERATOR>
|
||||
static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
|
||||
std::vector<Index> keys;
|
||||
static FastVector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
|
||||
FastVector<Index> keys;
|
||||
keys.reserve(n);
|
||||
for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
|
||||
return keys;
|
||||
|
|
|
@ -168,11 +168,11 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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
|
||||
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
|
||||
FastVector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
|
||||
#else
|
||||
vector<size_t> varDims(variableSlots.size());
|
||||
FastVector<size_t> varDims(variableSlots.size());
|
||||
#endif
|
||||
size_t m = 0;
|
||||
size_t n = 0;
|
||||
|
@ -221,7 +221,7 @@ break;
|
|||
|
||||
if (debug) cout << "Determine dimensions" << endl;
|
||||
tic(1, "countDims");
|
||||
vector<size_t> varDims;
|
||||
FastVector<size_t> varDims;
|
||||
size_t m, n;
|
||||
boost::tie(varDims, m, n) = countDims(factors, variableSlots);
|
||||
if (debug) {
|
||||
|
@ -233,7 +233,7 @@ break;
|
|||
|
||||
if (debug) cout << "Sort rows" << endl;
|
||||
tic(2, "sort rows");
|
||||
vector<JacobianFactor::_RowSource> rowSources;
|
||||
FastVector<JacobianFactor::_RowSource> rowSources;
|
||||
rowSources.reserve(m);
|
||||
bool anyConstrained = false;
|
||||
for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
|
||||
|
@ -367,7 +367,7 @@ break;
|
|||
|
||||
// Pull out keys and dimensions
|
||||
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) {
|
||||
dimensions[var_slot.second.slot] = var_slot.second.dimension;
|
||||
}
|
||||
|
@ -569,7 +569,7 @@ break;
|
|||
|
||||
// Pull out keys and dimensions
|
||||
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) {
|
||||
dimensions[var_slot.second.slot] = var_slot.second.dimension;
|
||||
}
|
||||
|
|
|
@ -21,8 +21,6 @@
|
|||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
@ -68,7 +66,7 @@ namespace gtsam {
|
|||
|
||||
// Allocate solution vector and copy RHS
|
||||
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);
|
||||
VectorValues result(dims);
|
||||
btreeRHS(rootClique, result);
|
||||
|
|
|
@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
|
||||
const vector<size_t>& dimensions, const Scatter& scatter) :
|
||||
const FastVector<size_t>& dimensions, const Scatter& scatter) :
|
||||
info_(matrix_) {
|
||||
|
||||
const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
|
||||
|
@ -505,7 +505,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
|
|||
tic(2, "remaining factor");
|
||||
info_.blockStart() = nrFrontals;
|
||||
// Assign the keys
|
||||
vector<Index> remainingKeys(keys_.size() - nrFrontals);
|
||||
FastVector<Index> remainingKeys(keys_.size() - nrFrontals);
|
||||
remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
|
||||
keys_.swap(remainingKeys);
|
||||
toc(2, "remaining factor");
|
||||
|
|
|
@ -204,7 +204,7 @@ namespace gtsam {
|
|||
|
||||
/** Special constructor used in EliminateCholesky which combines the given factors */
|
||||
HessianFactor(const FactorGraph<GaussianFactor>& factors,
|
||||
const std::vector<size_t>& dimensions, const Scatter& scatter);
|
||||
const FastVector<size_t>& dimensions, const Scatter& scatter);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~HessianFactor() {}
|
||||
|
|
|
@ -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) :
|
||||
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
|
||||
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
|
||||
|
@ -260,7 +260,7 @@ namespace gtsam {
|
|||
sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
|
||||
|
||||
// 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;
|
||||
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
|
||||
dimensions[j++] = Ab_(sourceSlot.second).cols();
|
||||
|
@ -269,7 +269,7 @@ namespace gtsam {
|
|||
dimensions.back() = 1;
|
||||
|
||||
// Copy the variables and matrix into the new order
|
||||
vector<Index> oldKeys(size());
|
||||
FastVector<Index> oldKeys(size());
|
||||
keys_.swap(oldKeys);
|
||||
AbMatrix oldMatrix;
|
||||
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();
|
||||
for (size_t row = 0; row < rows(); ++row) {
|
||||
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) {
|
||||
keys_.resize(variableSlots.size());
|
||||
std::transform(variableSlots.begin(), variableSlots.end(), begin(),
|
||||
|
@ -551,7 +551,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::copyFNZ(size_t m, size_t n,
|
||||
vector<_RowSource>& rowSources) {
|
||||
FastVector<_RowSource>& rowSources) {
|
||||
Index i = 0;
|
||||
for (size_t row = 0; row < m; ++row) {
|
||||
while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
|
||||
|
|
|
@ -85,7 +85,7 @@ namespace gtsam {
|
|||
protected:
|
||||
|
||||
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
|
||||
BlockAb Ab_; // the block view of the full matrix
|
||||
typedef GaussianFactor Base; // typedef to base
|
||||
|
@ -123,7 +123,7 @@ namespace gtsam {
|
|||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** 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);
|
||||
|
||||
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
|
||||
|
||||
/** 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 */
|
||||
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 */
|
||||
void copyRow(const JacobianFactor& source,
|
||||
Index sourceRow, size_t sourceSlot, size_t row, Index slot);
|
||||
|
||||
/** 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 */
|
||||
void setModel(bool anyConstrained, const Vector& sigmas);
|
||||
|
|
|
@ -163,7 +163,7 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
|
|||
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); }
|
||||
whitenInPlace(b);
|
||||
}
|
||||
|
@ -513,7 +513,7 @@ Vector Base::sqrtWeight(const Vector &error) const {
|
|||
* according to their weight implementation */
|
||||
|
||||
/** 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 ) {
|
||||
const double w = sqrtWeight(error.norm());
|
||||
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);
|
||||
}
|
||||
|
||||
void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
|
||||
void Robust::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
|
||||
noise_->WhitenSystem(A,b);
|
||||
robust_->reweight(A,b);
|
||||
}
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -76,7 +77,7 @@ namespace gtsam {
|
|||
|
||||
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& A1, Matrix& A2, 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
|
||||
*/
|
||||
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& A1, Matrix& A2, 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;
|
||||
|
||||
/** 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 &A1, Matrix &A2, 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
|
||||
|
||||
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& A1, Matrix& A2, Vector& b) const;
|
||||
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
|
||||
|
|
|
@ -57,7 +57,7 @@ namespace gtsam {
|
|||
if(recalculate) {
|
||||
|
||||
// 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;
|
||||
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
|
||||
originalValues[it - (*clique)->beginFrontals()] = delta[*it];
|
||||
|
|
|
@ -126,7 +126,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
|||
theta.insert(newTheta);
|
||||
if(debug) newTheta.print("The new variables are: ");
|
||||
// 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;
|
||||
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
|
||||
const size_t originalDim = delta->dim();
|
||||
|
@ -287,7 +287,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
|
||||
toc(2,"variable index");
|
||||
tic(3,"ccolamd");
|
||||
vector<int> cmember(affectedKeysSelector.size(), 0);
|
||||
FastVector<int> cmember(affectedKeysSelector.size(), 0);
|
||||
if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
|
||||
assert(reorderingMode.constrainedKeys);
|
||||
if(keys.size() > reorderingMode.constrainedKeys->size()) {
|
||||
|
|
|
@ -229,7 +229,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
|
|||
tic(1,"reorder");
|
||||
tic(1,"CCOLAMD");
|
||||
// Do a batch step - reorder and relinearize all variables
|
||||
vector<int> cmember(theta_.size(), 0);
|
||||
FastVector<int> cmember(theta_.size(), 0);
|
||||
FastSet<Index> constrainedKeysSet;
|
||||
if(constrainKeys)
|
||||
constrainedKeysSet = *constrainKeys;
|
||||
|
|
|
@ -24,7 +24,6 @@
|
|||
#include <limits>
|
||||
|
||||
#include <boost/serialization/base_object.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include <gtsam/inference/Factor-inl.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
|
@ -36,19 +35,6 @@
|
|||
|
||||
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
|
||||
|
@ -139,7 +125,7 @@ public:
|
|||
* variable indices.
|
||||
*/
|
||||
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)
|
||||
indices[j] = ordering[this->keys()[j]];
|
||||
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
|
||||
* 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
|
||||
|
@ -264,12 +250,12 @@ public:
|
|||
// Create the set of terms - Jacobians for each index
|
||||
Vector b;
|
||||
// Call evaluate error to get Jacobians and b vector
|
||||
std::vector<Matrix> A(this->size());
|
||||
FastVector<Matrix> A(this->size());
|
||||
b = -unwhitenedError(x, A);
|
||||
|
||||
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
|
||||
for(size_t j=0; j<this->size(); ++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
|
||||
* 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)) {
|
||||
const X& x1 = x.at<X>(keys_[0]);
|
||||
if(H) {
|
||||
|
@ -422,7 +408,7 @@ public:
|
|||
|
||||
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
||||
* 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)) {
|
||||
const X1& x1 = x.at<X1>(keys_[0]);
|
||||
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
|
||||
* 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(H)
|
||||
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
|
||||
* 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(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]);
|
||||
|
@ -707,7 +693,7 @@ public:
|
|||
|
||||
/** Calls the 5-key specific version of evaluateError, which is pure virtual
|
||||
* 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(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]);
|
||||
|
@ -813,7 +799,7 @@ public:
|
|||
|
||||
/** Calls the 6-key specific version of evaluateError, which is pure virtual
|
||||
* 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(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]);
|
||||
|
|
|
@ -59,29 +59,29 @@ TEST( GaussianJunctionTree, constructor2 )
|
|||
// create an ordering
|
||||
GaussianJunctionTree actual(fg);
|
||||
|
||||
vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
|
||||
vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
|
||||
vector<Index> frontal3; frontal3 += ordering["x1"];
|
||||
vector<Index> frontal4; frontal4 += ordering["x7"];
|
||||
vector<Index> sep1;
|
||||
vector<Index> sep2; sep2 += ordering["x4"];
|
||||
vector<Index> sep3; sep3 += ordering["x2"];
|
||||
vector<Index> sep4; sep4 += ordering["x6"];
|
||||
EXPECT(assert_equal(frontal1, actual.root()->frontal));
|
||||
EXPECT(assert_equal(sep1, actual.root()->separator));
|
||||
FastVector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
|
||||
FastVector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
|
||||
FastVector<Index> frontal3; frontal3 += ordering["x1"];
|
||||
FastVector<Index> frontal4; frontal4 += ordering["x7"];
|
||||
FastVector<Index> sep1;
|
||||
FastVector<Index> sep2; sep2 += ordering["x4"];
|
||||
FastVector<Index> sep3; sep3 += ordering["x2"];
|
||||
FastVector<Index> sep4; sep4 += ordering["x6"];
|
||||
EXPECT(assert_container_equality(frontal1, actual.root()->frontal));
|
||||
EXPECT(assert_container_equality(sep1, actual.root()->separator));
|
||||
LONGS_EQUAL(5, actual.root()->size());
|
||||
list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
|
||||
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
|
||||
GaussianJunctionTree::sharedClique child0 = *child0it;
|
||||
GaussianJunctionTree::sharedClique child1 = *child1it;
|
||||
EXPECT(assert_equal(frontal2, child0->frontal));
|
||||
EXPECT(assert_equal(sep2, child0->separator));
|
||||
EXPECT(assert_container_equality(frontal2, child0->frontal));
|
||||
EXPECT(assert_container_equality(sep2, child0->separator));
|
||||
LONGS_EQUAL(4, child0->size());
|
||||
EXPECT(assert_equal(frontal3, child0->children().front()->frontal));
|
||||
EXPECT(assert_equal(sep3, child0->children().front()->separator));
|
||||
EXPECT(assert_container_equality(frontal3, child0->children().front()->frontal));
|
||||
EXPECT(assert_container_equality(sep3, child0->children().front()->separator));
|
||||
LONGS_EQUAL(2, child0->children().front()->size());
|
||||
EXPECT(assert_equal(frontal4, child1->frontal));
|
||||
EXPECT(assert_equal(sep4, child1->separator));
|
||||
EXPECT(assert_container_equality(frontal4, child1->frontal));
|
||||
EXPECT(assert_container_equality(sep4, child1->separator));
|
||||
LONGS_EQUAL(2, child1->size());
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue