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

renamed varid_t to Index

release/4.3a0
Frank Dellaert 2010-10-11 21:14:35 +00:00
parent 96eb939749
commit 057050fa9f
68 changed files with 661 additions and 629 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
base/TestableAssertions.h
View File

@ -17,7 +17,7 @@ namespace gtsam {
/**
* Equals testing for basic types
*/
bool assert_equal(const varid_t& expected, const varid_t& actual, double tol = 0.0) {
bool assert_equal(const Index& expected, const Index& actual, double tol = 0.0) {
if(expected != actual) {
std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
return false;

67
base/types.h
View File

@ -11,37 +11,48 @@
namespace gtsam {
typedef size_t varid_t;
/**
* Integer variable index type
*/
typedef size_t Index;
/** Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not.
*/
template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector {};
/** Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not.
*/
template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
typename AS_CONST>
struct const_selector {
};
/** Specialization for the non-const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_NON_CONST type;
};
/** Specialization for the const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_CONST type;
};
/** Specialization for the non-const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_NON_CONST type;
};
/** Specialization for the const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_CONST type;
};
/**
* Helper struct that encapsulates a value with a default, this is just used
* as a member object so you don't have to specify defaults in the class
* constructor.
*/
template<typename T, T defaultValue>
struct ValueWithDefault {
T value;
ValueWithDefault() : value(defaultValue) {}
ValueWithDefault(const T& _value) : value(_value) {}
T& operator*() { return value; }
};
/**
* Helper struct that encapsulates a value with a default, this is just used
* as a member object so you don't have to specify defaults in the class
* constructor.
*/
template<typename T, T defaultValue>
struct ValueWithDefault {
T value;
ValueWithDefault() :
value(defaultValue) {
}
ValueWithDefault(const T& _value) :
value(_value) {
}
T& operator*() {
return value;
}
};
}

2
geometry/tensors.h
View File

@ -11,7 +11,7 @@ namespace tensors {
/** index */
template<int Dim, char C> struct Index {
enum { dim = Dim };
static const int dim = Dim;
};
} // namespace tensors

10
inference/BayesNet-inl.h
View File

@ -75,8 +75,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
list<varid_t> BayesNet<Conditional>::ordering() const {
list<varid_t> ord;
list<Index> BayesNet<Conditional>::ordering() const {
list<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.push_back(conditional->key());
return ord;
@ -88,8 +88,8 @@ namespace gtsam {
ofstream of(s.c_str());
of<< "digraph G{\n";
BOOST_FOREACH(const_sharedConditional conditional,conditionals_) {
varid_t child = conditional->key();
BOOST_FOREACH(varid_t parent, conditional->parents()) {
Index child = conditional->key();
BOOST_FOREACH(Index parent, conditional->parents()) {
of << parent << "->" << child << endl;
}
}
@ -101,7 +101,7 @@ namespace gtsam {
template<class Conditional>
typename BayesNet<Conditional>::sharedConditional
BayesNet<Conditional>::operator[](varid_t key) const {
BayesNet<Conditional>::operator[](Index key) const {
const_iterator it = find_if(conditionals_.begin(), conditionals_.end(), boost::lambda::bind(&Conditional::key, *boost::lambda::_1) == key);
if (it == conditionals_.end()) throw(invalid_argument((boost::format(
"BayesNet::operator['%1%']: not found") % key).str()));

4
inference/BayesNet.h
View File

@ -97,10 +97,10 @@ namespace gtsam {
}
/** return keys in reverse topological sort order, i.e., elimination order */
std::list<varid_t> ordering() const;
std::list<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](varid_t key) const;
sharedConditional operator[](Index key) const;
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }

68
inference/BayesTree-inl.h
View File

@ -76,8 +76,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
vector<varid_t> BayesTree<Conditional>::Clique::keys() const {
vector<varid_t> keys;
vector<Index> BayesTree<Conditional>::Clique::keys() const {
vector<Index> keys;
keys.reserve(this->size() + separator_.size());
BOOST_FOREACH(const sharedConditional conditional, *this) {
keys.push_back(conditional->key());
@ -92,7 +92,7 @@ namespace gtsam {
cout << s << "Clique ";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { cout << conditional->key() << " "; }
cout << "| ";
BOOST_FOREACH(const varid_t sep, separator_) { cout << sep << " "; }
BOOST_FOREACH(const Index sep, separator_) { cout << sep << " "; }
cout << "\n";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) {
conditional->print(" " + s + "conditional");
@ -100,7 +100,7 @@ namespace gtsam {
}
// if (!separator_.empty()) {
// cout << " :";
// BOOST_FOREACH(varid_t key, separator_)
// BOOST_FOREACH(Index key, separator_)
// cout << " " << key;
// }
// cout << endl;
@ -127,7 +127,7 @@ namespace gtsam {
template<class Conditional>
void BayesTree<Conditional>::Clique::permuteWithInverse(const Permutation& inversePermutation) {
BayesNet<Conditional>::permuteWithInverse(inversePermutation);
BOOST_FOREACH(varid_t& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
@ -140,14 +140,14 @@ namespace gtsam {
bool changed = BayesNet<Conditional>::permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(varid_t& separatorKey, separator_) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(Index& separatorKey, separator_) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(const shared_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
#endif
if(changed) {
BOOST_FOREACH(varid_t& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation);
@ -208,7 +208,7 @@ namespace gtsam {
}
first = true;
BOOST_FOREACH(varid_t sep, clique->separator_) {
BOOST_FOREACH(Index sep, clique->separator_) {
if(!first) parent += ","; first = false;
parent += (boost::format("%1%")%sep).str();
}
@ -285,28 +285,28 @@ namespace gtsam {
// However, an added wrinkle is that Cp might overlap with the root.
// Keys corresponding to the root should not be added to the ordering at all.
typedef set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > FastJSet;
typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > FastJSet;
// Get the key list Cp=Fp+Sp, which will form the basis for the integrands
vector<varid_t> parentKeys(parent->keys());
vector<Index> parentKeys(parent->keys());
FastJSet integrands(parentKeys.begin(), parentKeys.end());
// Start ordering with the separator
FastJSet separator(separator_.begin(), separator_.end());
// remove any variables in the root, after this integrands = Cp\R, ordering = S\R
BOOST_FOREACH(varid_t key, R->ordering()) {
BOOST_FOREACH(Index key, R->ordering()) {
integrands.erase(key);
separator.erase(key);
}
// remove any variables in the separator, after this integrands = Cp\R\S
BOOST_FOREACH(varid_t key, separator_) integrands.erase(key);
BOOST_FOREACH(Index key, separator_) integrands.erase(key);
// form the ordering as [Cp\R\S S\R]
vector<varid_t> ordering; ordering.reserve(integrands.size() + separator.size());
BOOST_FOREACH(varid_t key, integrands) ordering.push_back(key);
BOOST_FOREACH(varid_t key, separator) ordering.push_back(key);
vector<Index> ordering; ordering.reserve(integrands.size() + separator.size());
BOOST_FOREACH(Index key, integrands) ordering.push_back(key);
BOOST_FOREACH(Index key, separator) ordering.push_back(key);
// eliminate to get marginal
typename FactorGraph::variableindex_type varIndex(p_Cp_R);
@ -319,7 +319,7 @@ namespace gtsam {
BayesNet<Conditional> p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex);
// remove all integrands
for(varid_t j=0; j<integrands.size(); ++j)
for(Index j=0; j<integrands.size(); ++j)
p_S_R.pop_front();
// Undo the permutation on the shortcut
@ -370,7 +370,7 @@ namespace gtsam {
//
// // Find the keys of both C1 and C2
// Ordering keys12 = keys();
// BOOST_FOREACH(varid_t key,C2->keys()) keys12.push_back(key);
// BOOST_FOREACH(Index key,C2->keys()) keys12.push_back(key);
// keys12.unique();
//
// // Calculate the marginal
@ -396,7 +396,7 @@ namespace gtsam {
typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique(
const sharedConditional& conditional, sharedClique parent_clique) {
sharedClique new_clique(new Clique(conditional));
varid_t key = conditional->key();
Index key = conditional->key();
nodes_.resize(std::max(key+1, nodes_.size()));
nodes_[key] = new_clique;
if (parent_clique != NULL) {
@ -411,7 +411,7 @@ namespace gtsam {
typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique(
const sharedConditional& conditional, list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional));
varid_t key = conditional->key();
Index key = conditional->key();
nodes_.resize(max(key+1, nodes_.size()));
nodes_[key] = new_clique;
new_clique->children_ = child_cliques;
@ -428,7 +428,7 @@ namespace gtsam {
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(varid_t parent, conditional->parents()) {
BOOST_FOREACH(Index parent, conditional->parents()) {
assert(parent > conditional->key());
bool hasParent = false;
const Clique& cliquer(*clique);
@ -446,7 +446,7 @@ namespace gtsam {
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
varid_t key = conditional->key();
Index key = conditional->key();
nodes_.resize(std::max(key+1, nodes_.size()));
nodes_[key] = clique;
clique->push_front(conditional);
@ -466,7 +466,7 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename BayesTree<Conditional>::Clique::weak_ptr();
BOOST_FOREACH(varid_t key, clique->separator_) {
BOOST_FOREACH(Index key, clique->separator_) {
nodes_[key].reset();
}
const Clique& cliquer(*clique);
@ -547,14 +547,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
template<class Container>
inline varid_t BayesTree<Conditional>::findParentClique(const Container& parents) const {
inline Index BayesTree<Conditional>::findParentClique(const Container& parents) const {
typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
// boost::optional<varid_t> parentCliqueRepresentative;
// boost::optional<Index> parentCliqueRepresentative;
// boost::optional<size_t> lowest;
// BOOST_FOREACH(varid_t p, parents) {
// BOOST_FOREACH(Index p, parents) {
// size_t i = index(p);
// if (!lowest || i<*lowest) {
// lowest.reset(i);
@ -586,7 +586,7 @@ namespace gtsam {
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
varid_t parentRepresentative = findParentClique(parents);
Index parentRepresentative = findParentClique(parents);
if(debug) cout << "First-eliminated parent is " << parentRepresentative << endl;
sharedClique parent_clique = (*this)[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
@ -597,7 +597,7 @@ namespace gtsam {
#ifndef NDEBUG
// Debug check that the parent keys of the new conditional match the keys
// currently in the clique.
// list<varid_t>::const_iterator parent = parents.begin();
// list<Index>::const_iterator parent = parents.begin();
// typename Clique::const_iterator cond = parent_clique->begin();
// while(parent != parents.end()) {
// assert(cond != parent_clique->end());
@ -663,7 +663,7 @@ namespace gtsam {
assert(!root_);
root_ = subtree;
} else {
varid_t parentRepresentative = findParentClique(subtree->separator_);
Index parentRepresentative = findParentClique(subtree->separator_);
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += subtree;
subtree->parent_ = parent; // set new parent!
@ -682,7 +682,7 @@ namespace gtsam {
template<class Conditional>
template<class FactorGraph>
FactorGraph
BayesTree<Conditional>::marginal(varid_t key) const {
BayesTree<Conditional>::marginal(Index key) const {
// get clique containing key
sharedClique clique = (*this)[key];
@ -692,7 +692,7 @@ namespace gtsam {
// Reorder so that only the requested key is not eliminated
typename FactorGraph::variableindex_type varIndex(cliqueMarginal);
vector<varid_t> keyAsVector(1); keyAsVector[0] = key;
vector<Index> keyAsVector(1); keyAsVector[0] = key;
Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
Permutation::shared_ptr toBackInverse(toBack.inverse());
varIndex.permute(toBack);
@ -713,7 +713,7 @@ namespace gtsam {
template<class Conditional>
template<class FactorGraph>
BayesNet<Conditional>
BayesTree<Conditional>::marginalBayesNet(varid_t key) const {
BayesTree<Conditional>::marginalBayesNet(Index key) const {
// calculate marginal as a factor graph
FactorGraph fg = this->marginal<FactorGraph>(key);
@ -728,7 +728,7 @@ namespace gtsam {
// template<class Conditional>
// template<class Factor>
// FactorGraph<Factor>
// BayesTree<Conditional>::joint(varid_t key1, varid_t key2) const {
// BayesTree<Conditional>::joint(Index key1, Index key2) const {
//
// // get clique C1 and C2
// sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
@ -752,7 +752,7 @@ namespace gtsam {
// template<class Conditional>
// template<class Factor>
// BayesNet<Conditional>
// BayesTree<Conditional>::jointBayesNet(varid_t key1, varid_t key2) const {
// BayesTree<Conditional>::jointBayesNet(Index key1, Index key2) const {
//
// // calculate marginal as a factor graph
// FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
@ -803,7 +803,7 @@ namespace gtsam {
BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(const varid_t& key, keys) {
BOOST_FOREACH(const Index& key, keys) {
// get the clique
if(key < nodes_.size()) {

16
inference/BayesTree.h
View File

@ -45,7 +45,7 @@ namespace gtsam {
typedef typename boost::weak_ptr<Clique> weak_ptr;
weak_ptr parent_;
std::list<shared_ptr> children_;
std::list<varid_t> separator_; /** separator keys */
std::list<Index> separator_; /** separator keys */
typename Conditional::FactorType::shared_ptr cachedFactor_;
friend class BayesTree<Conditional>;
@ -58,7 +58,7 @@ namespace gtsam {
Clique(const BayesNet<Conditional>& bayesNet);
/** return keys in frontal:separator order */
std::vector<varid_t> keys() const;
std::vector<Index> keys() const;
/** print this node */
void print(const std::string& s = "") const;
@ -223,7 +223,7 @@ namespace gtsam {
* return the one with the lowest index in the ordering.
*/
template<class Container>
varid_t findParentClique(const Container& parents) const;
Index findParentClique(const Container& parents) const;
/** number of cliques */
inline size_t size() const {
@ -238,7 +238,7 @@ namespace gtsam {
sharedClique& root() { return root_; }
/** find the clique to which key belongs */
sharedClique operator[](varid_t key) const {
sharedClique operator[](Index key) const {
return nodes_.at(key);
}
@ -247,19 +247,19 @@ namespace gtsam {
/** return marginal on any variable */
template<class FactorGraph>
FactorGraph marginal(varid_t key) const;
FactorGraph marginal(Index key) const;
/** return marginal on any variable, as a Bayes Net */
template<class FactorGraph>
BayesNet<Conditional> marginalBayesNet(varid_t key) const;
BayesNet<Conditional> marginalBayesNet(Index key) const;
// /** return joint on two variables */
// template<class Factor>
// FactorGraph<Factor> joint(varid_t key1, varid_t key2) const;
// FactorGraph<Factor> joint(Index key1, Index key2) const;
//
// /** return joint on two variables as a BayesNet */
// template<class Factor>
// BayesNet<Conditional> jointBayesNet(varid_t key1, varid_t key2) const;
// BayesNet<Conditional> jointBayesNet(Index key1, Index key2) const;
/**
* Read only with side effects

6
inference/ClusterTree-inl.h
View File

@ -21,7 +21,7 @@ namespace gtsam {
* ************************************************************************* */
template<class FG>
template<class Iterator>
ClusterTree<FG>::Cluster::Cluster(const FG& fg, varid_t key, Iterator firstSeparator, Iterator lastSeparator) :
ClusterTree<FG>::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) :
FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
@ -63,10 +63,10 @@ namespace gtsam {
template<class FG>
void ClusterTree<FG>::Cluster::print(const string& indent) const {
cout << indent;
BOOST_FOREACH(const varid_t key, frontal)
BOOST_FOREACH(const Index key, frontal)
cout << key << " ";
cout << ": ";
BOOST_FOREACH(const varid_t key, separator)
BOOST_FOREACH(const Index key, separator)
cout << key << " ";
cout << endl;
}

6
inference/ClusterTree.h
View File

@ -36,8 +36,8 @@ namespace gtsam {
typedef typename boost::shared_ptr<Cluster> shared_ptr;
typedef typename boost::weak_ptr<Cluster> weak_ptr;
const std::vector<varid_t> frontal; // the frontal variables
const std::vector<varid_t> separator; // the separator variables
const std::vector<Index> frontal; // the frontal variables
const std::vector<Index> separator; // the separator variables
protected:
@ -52,7 +52,7 @@ namespace gtsam {
/* Create a node with a single frontal variable */
template<typename Iterator>
Cluster(const FG& fg, varid_t key, Iterator firstSeparator, Iterator lastSeparator);
Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
/* Create a node with several frontal variables */
template<typename FrontalIt, typename SeparatorIt>

24
inference/Conditional.h
View File

@ -56,19 +56,19 @@ public:
Conditional(){}
/** No parents */
Conditional(varid_t key) : factor_(key), nFrontal_(1) {}
Conditional(Index key) : factor_(key), nFrontal_(1) {}
/** Single parent */
Conditional(varid_t key, varid_t parent) : factor_(key, parent), nFrontal_(1) {}
Conditional(Index key, Index parent) : factor_(key, parent), nFrontal_(1) {}
/** Two parents */
Conditional(varid_t key, varid_t parent1, varid_t parent2) : factor_(key, parent1, parent2), nFrontal_(1) {}
Conditional(Index key, Index parent1, Index parent2) : factor_(key, parent1, parent2), nFrontal_(1) {}
/** Three parents */
Conditional(varid_t key, varid_t parent1, varid_t parent2, varid_t parent3) : factor_(key, parent1, parent2, parent3), nFrontal_(1) {}
Conditional(Index key, Index parent1, Index parent2, Index parent3) : factor_(key, parent1, parent2, parent3), nFrontal_(1) {}
/** Constructor from a frontal variable and a vector of parents */
Conditional(varid_t key, const std::vector<varid_t>& parents) : nFrontal_(1) {
Conditional(Index key, const std::vector<Index>& parents) : nFrontal_(1) {
factor_.keys_.resize(1+parents.size());
*(beginFrontals()) = key;
std::copy(parents.begin(), parents.end(), beginParents()); }
@ -78,7 +78,7 @@ public:
Conditional(Iterator firstKey, Iterator lastKey, size_t nFrontals) : factor_(firstKey, lastKey), nFrontal_(nFrontals) {}
/** Special accessor when there is only one frontal variable. */
varid_t key() const { assert(nFrontal_==1); return factor_.keys_[0]; }
Index key() const { assert(nFrontal_==1); return factor_.keys_[0]; }
/**
* Permutes the Conditional, but for efficiency requires the permutation
@ -93,11 +93,11 @@ public:
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(varid_t key, frontals()) { assert(key == inversePermutation[key]); }
BOOST_FOREACH(Index key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(varid_t& parent, parents()) {
varid_t newParent = inversePermutation[parent];
BOOST_FOREACH(Index& parent, parents()) {
Index newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
@ -107,7 +107,7 @@ public:
}
/** return a const reference to all keys */
const std::vector<varid_t>& keys() const { return factor_.keys(); }
const std::vector<Index>& keys() const { return factor_.keys(); }
/** return a view of the frontal keys */
Frontals frontals() const {
@ -126,9 +126,9 @@ public:
/** print */
void print(const std::string& s = "Conditional") const {
std::cout << s << " P(";
BOOST_FOREACH(varid_t key, frontals()) std::cout << " " << key;
BOOST_FOREACH(Index key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(varid_t parent, parents()) std::cout << " " << parent;
BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}

22
inference/EliminationTree-inl.h
View File

@ -19,7 +19,7 @@ namespace gtsam {
/* ************************************************************************* */
// template<class FG>
// void EliminationTree<FG>::add(const FG& fg, varid_t j) {
// void EliminationTree<FG>::add(const FG& fg, Index j) {
// sharedNode node(new Node(fg, j));
// add(node);
// }
@ -29,7 +29,7 @@ namespace gtsam {
void EliminationTree<FG>::add(const sharedNode& node) {
assert(node->frontal.size() == 1);
varid_t j = node->frontal.front();
Index j = node->frontal.front();
// Make a node and put it in the nodes_ array:
nodes_[j] = node;
@ -41,7 +41,7 @@ namespace gtsam {
else {
// find parent by iterating over all separator keys, and taking the lowest
// one in the ordering. That is the index of the parent clique.
vector<varid_t>::const_iterator parentIndex = min_element(node->separator.begin(), node->separator.end());
vector<Index>::const_iterator parentIndex = min_element(node->separator.begin(), node->separator.end());
assert(parentIndex != node->separator.end());
// attach to parent
sharedNode& parent = nodes_[*parentIndex];
@ -86,7 +86,7 @@ namespace gtsam {
// Build clusters
{
// Find highest-numbered variable
varid_t maxVar = 0;
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, fg) {
if(factor) {
typename FG::factor_type::const_iterator maxj = std::max_element(factor->begin(), factor->end());
@ -116,7 +116,7 @@ namespace gtsam {
// Loop over all variables and get factors that are connected
OrderedGraphs graphs;
Nodes nodesToAdd; nodesToAdd.reserve(columnIndex.size());
for(varid_t j=0; j<columnIndex.size(); ++j) {
for(Index j=0; j<columnIndex.size(); ++j) {
if(debug) cout << "Eliminating " << j << endl;
@ -138,8 +138,8 @@ namespace gtsam {
// this is stored as a vector of slot numbers, stored in order of the
// removed factors. The slot number is the max integer value if the
// factor does not involve that variable.
typedef map<varid_t, vector<varid_t> > VariableSlots;
map<varid_t, vector<varid_t> > variableSlots;
typedef map<Index, vector<Index> > VariableSlots;
map<Index, vector<Index> > variableSlots;
FG removedFactors; removedFactors.reserve(involvedFactors.size());
size_t jointFactorPos = 0;
BOOST_FOREACH(const size_t factorI, involvedFactors) {
@ -150,8 +150,8 @@ namespace gtsam {
removedFactors.push_back(fg[factorI]);
fg.remove(factorI);
varid_t factorVarSlot = 0;
BOOST_FOREACH(const varid_t involvedVariable, removedFactor.keys()) {
Index factorVarSlot = 0;
BOOST_FOREACH(const Index involvedVariable, removedFactor.keys()) {
// Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it
@ -159,10 +159,10 @@ namespace gtsam {
// we're combining. Initially we put the max integer value in
// the array entry for each factor that will indicate the factor
// does not involve the variable.
static vector<varid_t> empty;
static vector<Index> empty;
VariableSlots::iterator thisVarSlots = variableSlots.insert(make_pair(involvedVariable,empty)).first;
if(thisVarSlots->second.empty())
thisVarSlots->second.resize(involvedFactors.size(), numeric_limits<varid_t>::max());
thisVarSlots->second.resize(involvedFactors.size(), numeric_limits<Index>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor " << factorI << " slot " << factorVarSlot << endl;

4
inference/EliminationTree.h
View File

@ -31,7 +31,7 @@ template<class> class _EliminationTreeTester;
typedef typename Node::shared_ptr sharedNode;
// we typedef the following handy list of ordered factor graphs
typedef std::pair<varid_t, FG> NamedGraph;
typedef std::pair<Index, FG> NamedGraph;
typedef std::list<NamedGraph> OrderedGraphs;
private:
@ -47,7 +47,7 @@ template<class> class _EliminationTreeTester;
* add a factor graph fragment with given frontal key into the tree. Assumes
* parent node was already added (will throw exception if not).
*/
// void add(const FG& fg, varid_t key);
// void add(const FG& fg, Index key);
/**
* Add a pre-created node by computing its parent and children.

10
inference/Factor-inl.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
// keys_.resize(c->parents().size()+1);
// keys_[0] = c->key();
// size_t j = 1;
// BOOST_FOREACH(const varid_t parent, c->parents()) {
// BOOST_FOREACH(const Index parent, c->parents()) {
// keys_[j++] = parent;
// }
// checkSorted();
@ -41,19 +41,19 @@ template<class KeyIterator> Factor::Factor(KeyIterator beginKey, KeyIterator end
template<class FactorGraphType, class VariableIndexStorage>
Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<varid_t>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return shared_ptr(boost::make_shared<Factor>(variables.begin(), variables.end()));
}
/* ************************************************************************* */
template<class MapAllocator>
Factor::shared_ptr Factor::Combine(const FactorGraph<Factor>& factors, const std::map<varid_t, std::vector<varid_t>, std::less<varid_t>, MapAllocator>& variableSlots) {
typedef const std::map<varid_t, std::vector<varid_t>, std::less<varid_t>, MapAllocator> VariableSlots;
Factor::shared_ptr Factor::Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots) {
typedef const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator> VariableSlots;
typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter;
typedef boost::transform_iterator<
FirstGetter, typename VariableSlots::const_iterator,
varid_t, varid_t> IndexIterator;
Index, Index> IndexIterator;
FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1));
IndexIterator keysBegin(variableSlots.begin(), firstGetter);
IndexIterator keysEnd(variableSlots.end(), firstGetter);

10
inference/Factor.cpp
View File

@ -25,7 +25,7 @@ Factor::Factor(const Factor& f) : keys_(f.keys_), permuted_(f.permuted_) {}
/* ************************************************************************* */
void Factor::print(const std::string& s) const {
cout << s << " ";
BOOST_FOREACH(varid_t key, keys_) cout << " " << key;
BOOST_FOREACH(Index key, keys_) cout << " " << key;
cout << endl;
}
@ -38,18 +38,18 @@ bool Factor::equals(const Factor& other, double tol) const {
Conditional::shared_ptr Factor::eliminateFirst() {
assert(!keys_.empty());
checkSorted();
varid_t eliminated = keys_.front();
Index eliminated = keys_.front();
keys_.erase(keys_.begin());
return Conditional::shared_ptr(new Conditional(eliminated, keys_));
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(varid_t nFrontals) {
boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(Index nFrontals) {
assert(keys_.size() >= nFrontals);
checkSorted();
BayesNet<Conditional>::shared_ptr fragment(new BayesNet<Conditional>());
const_iterator nextFrontal = this->begin();
for(varid_t n = 0; n < nFrontals; ++n, ++nextFrontal)
for(Index n = 0; n < nFrontals; ++n, ++nextFrontal)
fragment->push_back(Conditional::shared_ptr(new Conditional(nextFrontal, const_iterator(this->end()), 1)));
if(nFrontals > 0)
keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents());
@ -59,7 +59,7 @@ boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(varid_t nFrontals) {
/* ************************************************************************* */
void Factor::permuteWithInverse(const Permutation& inversePermutation) {
this->permuted_.value = true;
BOOST_FOREACH(varid_t& key, keys_) { key = inversePermutation[key]; }
BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
}
}

28
inference/Factor.h
View File

@ -39,7 +39,7 @@ class Conditional;
class Factor : public Testable<Factor> {
protected:
std::vector<varid_t> keys_;
std::vector<Index> keys_;
ValueWithDefault<bool,true> permuted_;
/** Internal check to make sure keys are sorted (invariant during elimination).
@ -50,8 +50,8 @@ public:
typedef gtsam::Conditional Conditional;
typedef boost::shared_ptr<Factor> shared_ptr;
typedef std::vector<varid_t>::iterator iterator;
typedef std::vector<varid_t>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator;
typedef std::vector<Index>::const_iterator const_iterator;
/** Copy constructor */
Factor(const Factor& f);
@ -66,19 +66,19 @@ public:
Factor() : permuted_(false) {}
/** Construct unary factor */
Factor(varid_t key) : keys_(1), permuted_(false) {
Factor(Index key) : keys_(1), permuted_(false) {
keys_[0] = key; checkSorted(); }
/** Construct binary factor */
Factor(varid_t key1, varid_t key2) : keys_(2), permuted_(false) {
Factor(Index key1, Index key2) : keys_(2), permuted_(false) {
keys_[0] = key1; keys_[1] = key2; checkSorted(); }
/** Construct ternary factor */
Factor(varid_t key1, varid_t key2, varid_t key3) : keys_(3), permuted_(false) {
Factor(Index key1, Index key2, Index key3) : keys_(3), permuted_(false) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; checkSorted(); }
/** Construct 4-way factor */
Factor(varid_t key1, varid_t key2, varid_t key3, varid_t key4) : keys_(4), permuted_(false) {
Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4), permuted_(false) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; checkSorted(); }
/** Named constructor for combining a set of factors with pre-computed set of
@ -87,11 +87,11 @@ public:
template<class FactorGraphType, class VariableIndexStorage>
static shared_ptr Combine(const FactorGraphType& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<varid_t>& variables, const std::vector<std::vector<size_t> >& variablePositions);
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions);
/** Create a combined joint factor (new style for EliminationTree). */
template<class MapAllocator>
static shared_ptr Combine(const FactorGraph<Factor>& factors, const std::map<varid_t, std::vector<varid_t>, std::less<varid_t>, MapAllocator>& variableSlots);
static shared_ptr Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots);
/**
* eliminate the first variable involved in this factor
@ -102,7 +102,7 @@ public:
/**
* eliminate the first nFrontals frontal variables.
*/
boost::shared_ptr<BayesNet<Conditional> > eliminate(varid_t nFrontals = 1);
boost::shared_ptr<BayesNet<Conditional> > eliminate(Index nFrontals = 1);
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
@ -119,13 +119,13 @@ public:
iterator end() { return keys_.end(); }
/** First key*/
varid_t front() const { return keys_.front(); }
Index front() const { return keys_.front(); }
/** Last key */
varid_t back() const { return keys_.back(); }
Index back() const { return keys_.back(); }
/** find */
const_iterator find(varid_t key) const { return std::find(begin(), end(), key); }
const_iterator find(Index key) const { return std::find(begin(), end(), key); }
/** print */
void print(const std::string& s = "Factor") const;
@ -136,7 +136,7 @@ public:
/**
* return keys in order as created
*/
const std::vector<varid_t>& keys() const { return keys_; }
const std::vector<Index>& keys() const { return keys_; }
/**
* @return the number of nodes the factor connects

40
inference/FactorGraph-inl.h
View File

@ -105,8 +105,8 @@ namespace gtsam {
// * @param columns map from keys to a sparse column of non-zero row indices
// * @param lastKeys set of keys that should appear last in the ordering
// * ************************************************************************* */
// static void colamd(int n_col, int n_row, int nrNonZeros, const map<varid_t, vector<int> >& columns,
// Ordering& ordering, const set<varid_t>& lastKeys) {
// static void colamd(int n_col, int n_row, int nrNonZeros, const map<Index, vector<int> >& columns,
// Ordering& ordering, const set<Index>& lastKeys) {
//
// // Convert to compressed column major format colamd wants it in (== MATLAB format!)
// int Alen = ccolamd_recommended(nrNonZeros, n_row, n_col); /* colamd arg 3: size of the array A */
@ -117,11 +117,11 @@ namespace gtsam {
// p[0] = 0;
// int j = 1;
// int count = 0;
// typedef map<varid_t, vector<int> >::const_iterator iterator;
// typedef map<Index, vector<int> >::const_iterator iterator;
// bool front_exists = false;
// vector<varid_t> initialOrder;
// vector<Index> initialOrder;
// for (iterator it = columns.begin(); it != columns.end(); it++) {
// varid_t key = it->first;
// Index key = it->first;
// const vector<int>& column = it->second;
// initialOrder.push_back(key);
// BOOST_FOREACH(int i, column)
@ -159,12 +159,12 @@ namespace gtsam {
// /* ************************************************************************* */
// template<class Factor>
// void FactorGraph<Factor>::getOrdering(Ordering& ordering,
// const set<varid_t>& lastKeys,
// boost::optional<const set<varid_t>&> scope) const {
// const set<Index>& lastKeys,
// boost::optional<const set<Index>&> scope) const {
//
// // A factor graph is really laid out in row-major format, each factor a row
// // Below, we compute a symbolic matrix stored in sparse columns.
// map<varid_t, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
// map<Index, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
// int nrNonZeros = 0; // number of non-zero entries
// int n_row = 0; /* colamd arg 1: number of rows in A */
//
@ -173,9 +173,9 @@ namespace gtsam {
// bool hasInterested = scope.is_initialized();
// BOOST_FOREACH(const sharedFactor& factor, factors_) {
// if (factor == NULL) continue;
// const vector<varid_t>& keys(factor->keys());
// const vector<Index>& keys(factor->keys());
// inserted = false;
// BOOST_FOREACH(varid_t key, keys) {
// BOOST_FOREACH(Index key, keys) {
// if (!hasInterested || scope->find(key) != scope->end()) {
// columns[key].push_back(n_row);
// nrNonZeros++;
@ -192,7 +192,7 @@ namespace gtsam {
// template<class Factor>
// Ordering FactorGraph<Factor>::getOrdering() const {
// Ordering ordering;
// set<varid_t> lastKeys;
// set<Index> lastKeys;
// getOrdering(ordering, lastKeys);
// return ordering;
// }
@ -201,16 +201,16 @@ namespace gtsam {
// template<class Factor>
// boost::shared_ptr<Ordering> FactorGraph<Factor>::getOrdering_() const {
// boost::shared_ptr<Ordering> ordering(new Ordering);
// set<varid_t> lastKeys;
// set<Index> lastKeys;
// getOrdering(*ordering, lastKeys);
// return ordering;
// }
//
// /* ************************************************************************* */
// template<class Factor>
// Ordering FactorGraph<Factor>::getOrdering(const set<varid_t>& scope) const {
// Ordering FactorGraph<Factor>::getOrdering(const set<Index>& scope) const {
// Ordering ordering;
// set<varid_t> lastKeys;
// set<Index> lastKeys;
// getOrdering(ordering, lastKeys, scope);
// return ordering;
// }
@ -218,7 +218,7 @@ namespace gtsam {
// /* ************************************************************************* */
// template<class Factor>
// Ordering FactorGraph<Factor>::getConstrainedOrdering(
// const set<varid_t>& lastKeys) const {
// const set<Index>& lastKeys) const {
// Ordering ordering;
// getOrdering(ordering, lastKeys);
// return ordering;
@ -323,14 +323,14 @@ namespace gtsam {
// /* ************************************************************************* */
// template<class Factor>
// std::pair<FactorGraph<Factor> , set<varid_t> > FactorGraph<Factor>::removeSingletons() {
// std::pair<FactorGraph<Factor> , set<Index> > FactorGraph<Factor>::removeSingletons() {
// FactorGraph<Factor> singletonGraph;
// set<varid_t> singletons;
// set<Index> singletons;
//
// while (true) {
// // find all the singleton variables
// Ordering new_singletons;
// varid_t key;
// Index key;
// list<size_t> indices;
// BOOST_FOREACH(boost::tie(key, indices), indices_)
// {
@ -348,7 +348,7 @@ namespace gtsam {
// }
// singletons.insert(new_singletons.begin(), new_singletons.end());
//
// BOOST_FOREACH(const varid_t& singleton, new_singletons)
// BOOST_FOREACH(const Index& singleton, new_singletons)
// findAndRemoveFactors(singleton);
//
// // exit when there are no more singletons
@ -372,7 +372,7 @@ namespace gtsam {
// /* ************************************************************************* */
// template<class Factor> boost::shared_ptr<Factor> removeAndCombineFactors(
// FactorGraph<Factor>& factorGraph, const varid_t& key) {
// FactorGraph<Factor>& factorGraph, const Index& key) {
//
// // find and remove the factors associated with key
// vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);

14
inference/FactorGraph.h
View File

@ -49,8 +49,8 @@ namespace gtsam {
// * Return an ordering in first argument, potentially using a set of
// * keys that need to appear last, and potentially restricting scope
// */
// void getOrdering(Ordering& ordering, const std::set<varid_t>& lastKeys,
// boost::optional<const std::set<varid_t>&> scope = boost::none) const;
// void getOrdering(Ordering& ordering, const std::set<Index>& lastKeys,
// boost::optional<const std::set<Index>&> scope = boost::none) const;
public:
@ -117,8 +117,8 @@ namespace gtsam {
// */
// Ordering getOrdering() const;
// boost::shared_ptr<Ordering> getOrdering_() const;
// Ordering getOrdering(const std::set<varid_t>& scope) const;
// Ordering getConstrainedOrdering(const std::set<varid_t>& lastKeys) const;
// Ordering getOrdering(const std::set<Index>& scope) const;
// Ordering getConstrainedOrdering(const std::set<Index>& lastKeys) const;
// /**
// * find the minimum spanning tree using boost graph library
@ -167,10 +167,10 @@ namespace gtsam {
// * from the factor graph
// * @param key the key for the given node
// */
// std::vector<sharedFactor> findAndRemoveFactors(varid_t key);
// std::vector<sharedFactor> findAndRemoveFactors(Index key);
//
// /** remove singleton variables and the related factors */
// std::pair<FactorGraph<Factor>, std::set<varid_t> > removeSingletons();
// std::pair<FactorGraph<Factor>, std::set<Index> > removeSingletons();
private:
@ -198,7 +198,7 @@ namespace gtsam {
// * @return the combined linear factor
// */
// template<class Factor> boost::shared_ptr<Factor>
// removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const varid_t& key);
// removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Index& key);
/**

104
inference/ISAM2-inl.h
View File

@ -49,15 +49,15 @@ ISAM2<Conditional, Values>::ISAM2() : BayesTree<Conditional>(), delta_(Permutati
/* ************************************************************************* */
template<class Conditional, class Values>
list<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const list<varid_t>& keys) const {
list<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const list<Index>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const varid_t key, keys) { cout << key << " "; } }
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraph<NonlinearFactor<Values> > allAffected;
list<size_t> indices;
BOOST_FOREACH(const varid_t key, keys) {
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
// indices.insert(indices.begin(), l.begin(), l.end());
const VariableIndexType::mapped_type& factors(variableIndex_[key]);
@ -79,7 +79,7 @@ list<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const list<varid_t>&
// (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Values>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Values>::relinearizeAffectedFactors
(const list<varid_t>& affectedKeys) const {
(const list<Index>& affectedKeys) const {
tic("8.2.2.1 getAffectedFactors");
list<size_t> candidates = getAffectedFactors(affectedKeys);
@ -89,7 +89,7 @@ boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Values>::relinearizeAf
tic("8.2.2.2 affectedKeysSet");
// for fast lookup below
set<varid_t> affectedKeysSet;
set<Index> affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
toc("8.2.2.2 affectedKeysSet");
@ -97,7 +97,7 @@ boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Values>::relinearizeAf
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
varid_t var = ordering_[key];
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
break;
@ -122,12 +122,12 @@ GaussianFactorGraph ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that was eliminated
varid_t key = orphan->ordering().back();
Index key = orphan->ordering().back();
#ifndef NDEBUG
// typename BayesNet<Conditional>::const_iterator it = orphan->end();
// const Conditional& lastConditional = **(--it);
// typename Conditional::const_iterator keyit = lastConditional.endParents();
// const varid_t lastKey = *(--keyit);
// const Index lastKey = *(--keyit);
// assert(key == lastKey);
#endif
// retrieve the cached factor and add to boundary
@ -170,7 +170,7 @@ void reinsertCache(const typename ISAM2<Conditional,Values>::sharedClique& root,
}
template<class Conditional, class Values>
boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const set<varid_t>& markedKeys, const vector<varid_t>& newKeys, const GaussianFactorGraph* newFactors) {
boost::shared_ptr<set<Index> > ISAM2<Conditional, Values>::recalculate(const set<Index>& markedKeys, const vector<Index>& newKeys, const GaussianFactorGraph* newFactors) {
static const bool debug = false;
static const bool useMultiFrontal = true;
@ -198,7 +198,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
// if(debug) newFactors->print("Recalculating factors: ");
if(debug) {
cout << "markedKeys: ";
BOOST_FOREACH(const varid_t key, markedKeys) { cout << key << " "; }
BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
cout << endl;
}
@ -228,17 +228,17 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
tic("8.2 re-lookup");
// ordering provides all keys in conditionals, there cannot be others because path to root included
tic("8.2.1 re-lookup: affectedKeys");
list<varid_t> affectedKeys = affectedBayesNet.ordering();
list<Index> affectedKeys = affectedBayesNet.ordering();
toc("8.2.1 re-lookup: affectedKeys");
//#ifndef NDEBUG
// varid_t lastKey;
// for(list<varid_t>::const_iterator key=affectedKeys.begin(); key!=affectedKeys.end(); ++key) {
// Index lastKey;
// for(list<Index>::const_iterator key=affectedKeys.begin(); key!=affectedKeys.end(); ++key) {
// if(key != affectedKeys.begin())
// assert(*key > lastKey);
// lastKey = *key;
// }
//#endif
list<varid_t> affectedAndNewKeys;
list<Index> affectedAndNewKeys;
affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
tic("8.2.2 re-lookup: relinearizeAffected");
@ -250,7 +250,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
// The relinearized variables should not appear anywhere in the orphans
BOOST_FOREACH(boost::shared_ptr<const typename BayesTree<Conditional>::Clique> clique, orphans) {
BOOST_FOREACH(const typename GaussianConditional::shared_ptr& cond, *clique) {
BOOST_FOREACH(const varid_t key, cond->keys()) {
BOOST_FOREACH(const Index key, cond->keys()) {
assert(lastRelinVariables_[key] == false);
}
}
@ -259,7 +259,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
#endif
// if(debug) factors.print("Affected factors: ");
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const varid_t key, affectedKeys) { cout << key << " "; } cout << endl; }
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeys.size();
@ -275,7 +275,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
toc("8.2 re-lookup");
//#ifndef NDEBUG
// for(varid_t var=0; var<cached_.size(); ++var) {
// for(Index var=0; var<cached_.size(); ++var) {
// if(find(affectedKeys.begin(), affectedKeys.end(), var) == affectedKeys.end() ||
// lastRelinVariables_[var] == true) {
// assert(!cached_[var] || find(cached_[var]->begin(), cached_[var]->end(), var) == cached_[var]->end());
@ -292,7 +292,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) {
#ifndef NDEBUG
#ifndef SEPARATE_STEPS
BOOST_FOREACH(const varid_t key, *cached) {
BOOST_FOREACH(const Index key, *cached) {
assert(lastRelinVariables_[key] == false);
}
#endif
@ -331,13 +331,13 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
tic("8.5.1 re-order: select affected variables");
// create a partial reordering for the new and contaminated factors
// markedKeys are passed in: those variables will be forced to the end in the ordering
boost::shared_ptr<set<varid_t> > affectedKeysSet(new set<varid_t>(markedKeys));
boost::shared_ptr<set<Index> > affectedKeysSet(new set<Index>(markedKeys));
affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
//#ifndef NDEBUG
// // All affected keys should be contiguous and at the end of the elimination order
// for(set<varid_t>::const_iterator key=affectedKeysSet->begin(); key!=affectedKeysSet->end(); ++key) {
// for(set<Index>::const_iterator key=affectedKeysSet->begin(); key!=affectedKeysSet->end(); ++key) {
// if(key != affectedKeysSet->begin()) {
// set<varid_t>::const_iterator prev = key; --prev;
// set<Index>::const_iterator prev = key; --prev;
// assert(*prev == *key - 1);
// }
// }
@ -348,7 +348,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
// Debug check that all variables involved in the factors to be re-eliminated
// are in affectedKeys, since we will use it to select a subset of variables.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(varid_t key, factor->keys()) {
BOOST_FOREACH(Index key, factor->keys()) {
assert(find(affectedKeysSet->begin(), affectedKeysSet->end(), key) != affectedKeysSet->end());
}
}
@ -357,9 +357,9 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
Permutation affectedKeysSelectorInverse(affectedKeysSet->size() > 0 ? *(--affectedKeysSet->end())+1 : 0 /*ordering_.nVars()*/); // And its inverse
#ifndef NDEBUG
// If debugging, fill with invalid values that will trip asserts if dereferenced
std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits<varid_t>::max());
std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits<Index>::max());
#endif
{ varid_t position=0; BOOST_FOREACH(varid_t var, *affectedKeysSet) {
{ Index position=0; BOOST_FOREACH(Index var, *affectedKeysSet) {
affectedKeysSelector[position] = var;
affectedKeysSelectorInverse[var] = position;
++ position; } }
@ -384,28 +384,28 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
#ifdef PRESORT_ALPHA
Permutation alphaOrder(affectedKeysSet->size());
vector<Symbol> orderedKeys; orderedKeys.reserve(ordering_.size());
varid_t alphaVar = 0;
Index alphaVar = 0;
BOOST_FOREACH(const Ordering::value_type& key_order, ordering_) {
Permutation::const_iterator selected = find(affectedKeysSelector.begin(), affectedKeysSelector.end(), key_order.second);
if(selected != affectedKeysSelector.end()) {
varid_t selectedVar = selected - affectedKeysSelector.begin();
Index selectedVar = selected - affectedKeysSelector.begin();
alphaOrder[alphaVar] = selectedVar;
++ alphaVar;
}
}
assert(alphaVar == affectedKeysSet->size());
vector<varid_t> markedKeysSelected; markedKeysSelected.reserve(markedKeys.size());
BOOST_FOREACH(varid_t var, markedKeys) { markedKeysSelected.push_back(alphaOrder[affectedKeysSelectorInverse[var]]); }
vector<Index> markedKeysSelected; markedKeysSelected.reserve(markedKeys.size());
BOOST_FOREACH(Index var, markedKeys) { markedKeysSelected.push_back(alphaOrder[affectedKeysSelectorInverse[var]]); }
GaussianVariableIndex<> origAffectedFactorsIndex(affectedFactorsIndex);
affectedFactorsIndex.permute(alphaOrder);
Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD(affectedFactorsIndex, markedKeysSelected));
affectedFactorsIndex.permute(*alphaOrder.inverse());
affectedColamd = alphaOrder.permute(*affectedColamd);
#else
// vector<varid_t> markedKeysSelected; markedKeysSelected.reserve(markedKeys.size());
// BOOST_FOREACH(varid_t var, markedKeys) { markedKeysSelected.push_back(affectedKeysSelectorInverse[var]); }
vector<varid_t> newKeysSelected; newKeysSelected.reserve(newKeys.size());
BOOST_FOREACH(varid_t var, newKeys) { newKeysSelected.push_back(affectedKeysSelectorInverse[var]); }
// vector<Index> markedKeysSelected; markedKeysSelected.reserve(markedKeys.size());
// BOOST_FOREACH(Index var, markedKeys) { markedKeysSelected.push_back(affectedKeysSelectorInverse[var]); }
vector<Index> newKeysSelected; newKeysSelected.reserve(newKeys.size());
BOOST_FOREACH(Index var, newKeys) { newKeysSelected.push_back(affectedKeysSelectorInverse[var]); }
Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD(affectedFactorsIndex, newKeysSelected));
if(disableReordering) {
affectedColamd.reset(new Permutation(Permutation::Identity(affectedKeysSelector.size())));
@ -487,7 +487,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
tic("8.6 eliminate");
boost::shared_ptr<GaussianBayesNet> bayesNet(new GaussianBayesNet());
vector<GaussianFactor::shared_ptr> newlyCached(affectedKeysSelector.size());
for(varid_t var=0; var<affectedKeysSelector.size(); ++var) {
for(Index var=0; var<affectedKeysSelector.size(); ++var) {
GaussianConditional::shared_ptr conditional = Inference::EliminateOne(factors, affectedFactorsIndex, var);
// assert(partialReordering[affectedKeysSelector[var]] == affectedKeysSelectorInverse[affectedColamd[var]]);
// assert(reorderedSelectorInverse[var] == partialReordering[affectedKeysSelector[var]]);
@ -547,7 +547,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
// will be sorted correctly according to the new elimination order after
// applying the permutation, so findParentClique, which looks for the
// lowest-ordered parent, will still work.
varid_t parentRepresentative = findParentClique(orphan->separator_);
Index parentRepresentative = findParentClique(orphan->separator_);
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += orphan;
orphan->parent_ = parent; // set new parent!
@ -557,7 +557,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
// Output: BayesTree(this)
// boost::shared_ptr<set<varid_t> > affectedKeysSet(new set<varid_t>());
// boost::shared_ptr<set<Index> > affectedKeysSet(new set<Index>());
// affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
return affectedKeysSet;
}
@ -565,17 +565,17 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const s
///* ************************************************************************* */
//template<class Conditional, class Values>
//void ISAM2<Conditional, Values>::linear_update(const GaussianFactorGraph& newFactors) {
// const list<varid_t> markedKeys = newFactors.keys();
// const list<Index> markedKeys = newFactors.keys();
// recalculate(markedKeys, &newFactors);
//}
/* ************************************************************************* */
// find all variables that are directly connected by a measurement to one of the marked variables
template<class Conditional, class Values>
void ISAM2<Conditional, Values>::find_all(sharedClique clique, set<varid_t>& keys, const vector<bool>& markedMask) {
void ISAM2<Conditional, Values>::find_all(sharedClique clique, set<Index>& keys, const vector<bool>& markedMask) {
// does the separator contain any of the variables?
bool found = false;
BOOST_FOREACH(const varid_t& key, clique->separator_) {
BOOST_FOREACH(const Index& key, clique->separator_) {
if (markedMask[key])
found = true;
}
@ -598,7 +598,7 @@ struct _SelectiveExpmap {
delta(_delta), ordering(_ordering), mask(_mask) {}
template<typename I>
void operator()(I it_x) {
varid_t var = ordering[it_x->first];
Index var = ordering[it_x->first];
assert(delta[var].size() == it_x->second.dim());
if(mask[var]) it_x->second = it_x->second.expmap(delta[var]); }
};
@ -611,7 +611,7 @@ struct _SelectiveExpmapAndClear {
delta(_delta), ordering(_ordering), mask(_mask) {}
template<typename I>
void operator()(I it_x) {
varid_t var = ordering[it_x->first];
Index var = ordering[it_x->first];
assert(delta[var].size() == it_x->second.dim());
BOOST_FOREACH(double v, delta[var]) assert(isfinite(v));
if(disableReordering) {
@ -632,7 +632,7 @@ struct _VariableAdder {
template<typename I>
void operator()(I xIt) {
static const bool debug = false;
varid_t var = vconfig->push_back_preallocated(zero(xIt->second.dim()));
Index var = vconfig->push_back_preallocated(zero(xIt->second.dim()));
vconfig.permutation()[var] = var;
ordering.insert(xIt->first, var);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl;
@ -668,7 +668,7 @@ void ISAM2<Conditional, Values>::update(
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
vector<varid_t> dims(newTheta.dims(*newTheta.orderingArbitrary(ordering_.nVars())));
vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary(ordering_.nVars())));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
delta_.container().reserve(delta_->size() + newTheta.size(), delta_->dim() + accumulate(dims.begin(), dims.end(), 0));
delta_.permutation().resize(delta_->size() + newTheta.size());
@ -688,15 +688,15 @@ void ISAM2<Conditional, Values>::update(
tic("3 step3");
// 3. Mark linear update
set<varid_t> markedKeys;
vector<varid_t> newKeys; newKeys.reserve(newFactors.size() * 6);
set<Index> markedKeys;
vector<Index> newKeys; newKeys.reserve(newFactors.size() * 6);
BOOST_FOREACH(const typename NonlinearFactor<Values>::shared_ptr& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
markedKeys.insert(ordering_[key]);
newKeys.push_back(ordering_[key]);
}
}
// list<varid_t> markedKeys = newFactors.keys();
// list<Index> markedKeys = newFactors.keys();
toc("3 step3");
#ifdef SEPARATE_STEPS // original algorithm from paper: separate relin and optimize
@ -705,7 +705,7 @@ void ISAM2<Conditional, Values>::update(
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_, ordering_);
variableIndex_.augment(*linearFactors);
boost::shared_ptr<set<varid_t> > replacedKeys_todo = recalculate(markedKeys, newKeys, &(*linearFactors));
boost::shared_ptr<set<Index> > replacedKeys_todo = recalculate(markedKeys, newKeys, &(*linearFactors));
markedKeys.clear();
vector<bool> none(variableIndex_.size(), false);
optimize2(this->root(), wildfire_threshold, none, delta_);
@ -716,7 +716,7 @@ void ISAM2<Conditional, Values>::update(
if (relinearize && count%10 == 0) { // todo: every n steps
tic("4 step4");
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
for(varid_t var=0; var<delta_.size(); ++var) {
for(Index var=0; var<delta_.size(); ++var) {
if (max(abs(delta_[var])) >= relinearize_threshold) {
markedRelinMask[var] = true;
markedKeys.insert(var);
@ -742,7 +742,7 @@ void ISAM2<Conditional, Values>::update(
//markedKeys.splice(markedKeys.begin(), affectedKeys, affectedKeys.begin(), affectedKeys.end());
//markedKeys.sort(); // remove duplicates
//markedKeys.unique();
// BOOST_FOREACH(const varid_t var, affectedKeys) {
// BOOST_FOREACH(const Index var, affectedKeys) {
// markedKeys.push_back(var);
// }
toc("5 step5");
@ -788,11 +788,11 @@ void ISAM2<Conditional, Values>::update(
tic("8 step8");
// 8. Redo top of Bayes tree
boost::shared_ptr<set<varid_t> > replacedKeys = recalculate(markedKeys, newKeys, &(*linearFactors));
boost::shared_ptr<set<Index> > replacedKeys = recalculate(markedKeys, newKeys, &(*linearFactors));
toc("8 step8");
#else
vector<varid_t> empty;
boost::shared_ptr<set<varid_t> > replacedKeys = recalculate(markedKeys, empty);
vector<Index> empty;
boost::shared_ptr<set<Index> > replacedKeys = recalculate(markedKeys, empty);
#endif
tic("9 step9");
@ -812,7 +812,7 @@ void ISAM2<Conditional, Values>::update(
} else {
vector<bool> replacedKeysMask(variableIndex_.size(), false);
BOOST_FOREACH(const varid_t var, *replacedKeys) { replacedKeysMask[var] = true; }
BOOST_FOREACH(const Index var, *replacedKeys) { replacedKeysMask[var] = true; }
lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, replacedKeysMask, delta_); // modifies delta_
}
toc("9 step9");

10
inference/ISAM2.h
View File

@ -52,7 +52,7 @@ protected:
// for keeping all original nonlinear factors
NonlinearFactorGraph<Values> nonlinearFactors_;
// The "ordering" allows converting Symbols to varid_t (integer) keys. We
// The "ordering" allows converting Symbols to Index (integer) keys. We
// keep it up to date as we add and reorder variables.
Ordering ordering_;
@ -108,13 +108,13 @@ public:
private:
std::list<size_t> getAffectedFactors(const std::list<varid_t>& keys) const;
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::list<varid_t>& affectedKeys) const;
std::list<size_t> getAffectedFactors(const std::list<Index>& keys) const;
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::list<Index>& affectedKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
boost::shared_ptr<std::set<varid_t> > recalculate(const std::set<varid_t>& markedKeys, const std::vector<varid_t>& newKeys, const GaussianFactorGraph* newFactors = NULL);
boost::shared_ptr<std::set<Index> > recalculate(const std::set<Index>& markedKeys, const std::vector<Index>& newKeys, const GaussianFactorGraph* newFactors = NULL);
// void linear_update(const GaussianFactorGraph& newFactors);
void find_all(sharedClique clique, std::set<varid_t>& keys, const std::vector<bool>& marked); // helper function
void find_all(sharedClique clique, std::set<Index>& keys, const std::vector<bool>& marked); // helper function
}; // ISAM2

18
inference/JunctionTree-inl.h
View File

@ -57,13 +57,13 @@ 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<varid_t> lowestOrdered(fg.size());
varid_t maxVar = 0;
vector<Index> lowestOrdered(fg.size());
Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) {
typename FG::factor_type::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min == fg[i]->end())
lowestOrdered[i] = numeric_limits<varid_t>::max();
lowestOrdered[i] = numeric_limits<Index>::max();
else {
lowestOrdered[i] = *min;
maxVar = std::max(maxVar, *min);
@ -74,7 +74,7 @@ namespace gtsam {
// variable.
vector<list<size_t, boost::fast_pool_allocator<size_t> > > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != numeric_limits<varid_t>::max())
if(lowestOrdered[i] != numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i);
// Now call the recursive distributeFactors
@ -89,13 +89,13 @@ namespace gtsam {
if(bayesClique) {
// create a new clique in the junction tree
list<varid_t> frontals = bayesClique->ordering();
list<Index> frontals = bayesClique->ordering();
sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
// count the factors for this cluster to pre-allocate space
{
size_t nFactors = 0;
BOOST_FOREACH(const varid_t frontal, clique->frontal) {
BOOST_FOREACH(const Index frontal, clique->frontal) {
// There may be less variables in "targets" than there really are if
// some of the highest-numbered variables do not pull in any factors.
if(frontal < targets.size())
@ -103,7 +103,7 @@ namespace gtsam {
clique->reserve(nFactors);
}
// add the factors to this cluster
BOOST_FOREACH(const varid_t frontal, clique->frontal) {
BOOST_FOREACH(const Index frontal, clique->frontal) {
if(frontal < targets.size()) {
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
clique->push_back(fg[factorI]); } } }
@ -145,10 +145,10 @@ namespace gtsam {
#ifndef NDEBUG
// Debug check that the keys found in the factors match the frontal and
// separator keys of the clique.
list<varid_t> allKeys;
list<Index> allKeys;
allKeys.insert(allKeys.end(), current->frontal.begin(), current->frontal.end());
allKeys.insert(allKeys.end(), current->separator.begin(), current->separator.end());
vector<varid_t> varslotsKeys(variableSlots.size());
vector<Index> varslotsKeys(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), varslotsKeys.begin(),
boost::lambda::bind(&VariableSlots::iterator::value_type::first, boost::lambda::_1));
assert(std::equal(allKeys.begin(), allKeys.end(), varslotsKeys.begin()));

26
inference/Permutation.cpp
View File

@ -15,15 +15,15 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Permutation Permutation::Identity(varid_t nVars) {
Permutation Permutation::Identity(Index nVars) {
Permutation ret(nVars);
for(varid_t i=0; i<nVars; ++i)
for(Index i=0; i<nVars; ++i)
ret[i] = i;
return ret;
}
/* ************************************************************************* */
Permutation Permutation::PullToFront(const vector<varid_t>& toFront, size_t size) {
Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size) {
Permutation ret(size);
@ -32,14 +32,14 @@ Permutation Permutation::PullToFront(const vector<varid_t>& toFront, size_t size
// Put the pulled variables at the front of the permutation and set up the
// pulled flags.
for(varid_t j=0; j<toFront.size(); ++j) {
for(Index j=0; j<toFront.size(); ++j) {
ret[j] = toFront[j];
pulled[toFront[j]] = true;
}
// Fill in the rest of the variables
varid_t nextVar = toFront.size();
for(varid_t j=0; j<size; ++j)
Index nextVar = toFront.size();
for(Index j=0; j<size; ++j)
if(!pulled[j])
ret[nextVar++] = j;
assert(nextVar == size);
@ -48,7 +48,7 @@ Permutation Permutation::PullToFront(const vector<varid_t>& toFront, size_t size
}
/* ************************************************************************* */
Permutation Permutation::PushToBack(const std::vector<varid_t>& toBack, size_t size) {
Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t size) {
Permutation ret(size);
@ -57,8 +57,8 @@ Permutation Permutation::PushToBack(const std::vector<varid_t>& toBack, size_t s
// Put the pushed variables at the back of the permutation and set up the
// pushed flags;
varid_t nextVar = size - toBack.size();
for(varid_t j=0; j<toBack.size(); ++j) {
Index nextVar = size - toBack.size();
for(Index j=0; j<toBack.size(); ++j) {
ret[nextVar++] = toBack[j];
pushed[toBack[j]] = true;
}
@ -66,7 +66,7 @@ Permutation Permutation::PushToBack(const std::vector<varid_t>& toBack, size_t s
// Fill in the rest of the variables
nextVar = 0;
for(varid_t j=0; j<size; ++j)
for(Index j=0; j<size; ++j)
if(!pushed[j])
ret[nextVar++] = j;
assert(nextVar == size - toBack.size());
@ -91,7 +91,7 @@ Permutation::shared_ptr Permutation::partialPermutation(
assert(selector.size() == partialPermutation.size());
Permutation::shared_ptr result(new Permutation(*this));
for(varid_t subsetPos=0; subsetPos<selector.size(); ++subsetPos)
for(Index subsetPos=0; subsetPos<selector.size(); ++subsetPos)
(*result)[selector[subsetPos]] = (*this)[selector[partialPermutation[subsetPos]]];
return result;
@ -100,7 +100,7 @@ Permutation::shared_ptr Permutation::partialPermutation(
/* ************************************************************************* */
Permutation::shared_ptr Permutation::inverse() const {
Permutation::shared_ptr result(new Permutation(this->size()));
for(varid_t i=0; i<this->size(); ++i) {
for(Index i=0; i<this->size(); ++i) {
assert((*this)[i] < this->size());
(*result)[(*this)[i]] = i;
}
@ -110,7 +110,7 @@ Permutation::shared_ptr Permutation::inverse() const {
/* ************************************************************************* */
void Permutation::print(const std::string& str) const {
std::cout << str;
BOOST_FOREACH(varid_t s, rangeIndices_) { std::cout << s << " "; }
BOOST_FOREACH(Index s, rangeIndices_) { std::cout << s << " "; }
std::cout << std::endl;
}

22
inference/Permutation.h
View File

@ -34,12 +34,12 @@ class Inference;
*/
class Permutation {
protected:
std::vector<varid_t> rangeIndices_;
std::vector<Index> rangeIndices_;
public:
typedef boost::shared_ptr<Permutation> shared_ptr;
typedef std::vector<varid_t>::const_iterator const_iterator;
typedef std::vector<varid_t>::iterator iterator;
typedef std::vector<Index>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator;
/**
* Create an empty permutation. This cannot do anything, but you can later
@ -51,20 +51,20 @@ public:
* Create an uninitialized permutation. You must assign all values using the
* square bracket [] operator or they will be undefined!
*/
Permutation(varid_t nVars) : rangeIndices_(nVars) {}
Permutation(Index nVars) : rangeIndices_(nVars) {}
/**
* Permute the given variable, i.e. determine it's new index after the
* permutation.
*/
varid_t operator[](varid_t variable) const { check(variable); return rangeIndices_[variable]; }
varid_t& operator[](varid_t variable) { check(variable); return rangeIndices_[variable]; }
Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; }
Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; }
/**
* The number of variables in the range of this permutation, i.e. the output
* space.
*/
varid_t size() const { return rangeIndices_.size(); }
Index size() const { return rangeIndices_.size(); }
/** Whether the permutation contains any entries */
bool empty() const { return rangeIndices_.empty(); }
@ -79,19 +79,19 @@ public:
/**
* Return an identity permutation.
*/
static Permutation Identity(varid_t nVars);
static Permutation Identity(Index nVars);
/**
* Create a permutation that pulls the given variables to the front while
* pushing the rest to the back.
*/
static Permutation PullToFront(const std::vector<varid_t>& toFront, size_t size);
static Permutation PullToFront(const std::vector<Index>& toFront, size_t size);
/**
* Create a permutation that pulls the given variables to the front while
* pushing the rest to the back.
*/
static Permutation PushToBack(const std::vector<varid_t>& toBack, size_t size);
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size);
iterator begin() { return rangeIndices_.begin(); }
const_iterator begin() const { return rangeIndices_.begin(); }
@ -136,7 +136,7 @@ public:
Permutation::shared_ptr inverse() const;
protected:
void check(varid_t variable) const { assert(variable < rangeIndices_.size()); }
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
friend class Inference;
};

12
inference/SymbolicFactorGraph.cpp
View File

@ -27,33 +27,33 @@ namespace gtsam {
FactorGraph<Factor>(bayesNet) {}
/* ************************************************************************* */
void SymbolicFactorGraph::push_factor(varid_t key) {
void SymbolicFactorGraph::push_factor(Index key) {
boost::shared_ptr<Factor> factor(new Factor(key));
push_back(factor);
}
/** Push back binary factor */
void SymbolicFactorGraph::push_factor(varid_t key1, varid_t key2) {
void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2));
push_back(factor);
}
/** Push back ternary factor */
void SymbolicFactorGraph::push_factor(varid_t key1, varid_t key2, varid_t key3) {
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2,key3));
push_back(factor);
}
/** Push back 4-way factor */
void SymbolicFactorGraph::push_factor(varid_t key1, varid_t key2, varid_t key3, varid_t key4) {
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2,key3,key4));
push_back(factor);
}
/* ************************************************************************* */
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> >
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> >
SymbolicFactorGraph::keys() const {
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > keys;
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;

10
inference/SymbolicFactorGraph.h
View File

@ -32,16 +32,16 @@ public:
SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet);
/** Push back unary factor */
void push_factor(varid_t key);
void push_factor(Index key);
/** Push back binary factor */
void push_factor(varid_t key1, varid_t key2);
void push_factor(Index key1, Index key2);
/** Push back ternary factor */
void push_factor(varid_t key1, varid_t key2, varid_t key3);
void push_factor(Index key1, Index key2, Index key3);
/** Push back 4-way factor */
void push_factor(varid_t key1, varid_t key2, varid_t key3, varid_t key4);
void push_factor(Index key1, Index key2, Index key3, Index key4);
/**
* Construct from a factor graph of any type
@ -53,7 +53,7 @@ public:
* Return the set of variables involved in the factors (computes a set
* union).
*/
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > keys() const;
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys() const;
/**
* Same as eliminate in the SymbolicFactorGraph case

48
inference/VariableIndex.h
View File

@ -63,11 +63,11 @@ public:
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
varid_t size() const { return index_.size(); }
Index size() const { return index_.size(); }
size_t nFactors() const { return nFactors_; }
size_t nEntries() const { return nEntries_; }
const mapped_type& operator[](varid_t variable) const { checkVar(variable); return index_[variable]; }
mapped_type& operator[](varid_t variable) { checkVar(variable); return index_[variable]; }
const mapped_type& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
mapped_type& operator[](Index variable) { checkVar(variable); return index_[variable]; }
void permute(const Permutation& permutation);
// template<class Derived> void augment(const Derived& varIndex);
template<class FactorGraph> void augment(const FactorGraph& factorGraph);
@ -78,12 +78,12 @@ public:
protected:
VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
void checkVar(varid_t variable) const { assert(variable < index_.size()); }
void checkVar(Index variable) const { assert(variable < index_.size()); }
factor_iterator factorsBegin(varid_t variable) { checkVar(variable); return index_[variable].begin(); }
const_factor_iterator factorsBegin(varid_t variable) const { checkVar(variable); return index_[variable].begin(); }
factor_iterator factorsEnd(varid_t variable) { checkVar(variable); return index_[variable].end(); }
const_factor_iterator factorsEnd(varid_t variable) const { checkVar(variable); return index_[variable].end(); }
factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
const_factor_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
const_factor_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
friend class Inference;
};
@ -94,14 +94,14 @@ void VariableIndex<Storage>::permute(const Permutation& permutation) {
#ifndef NDEBUG
// Assert that the permutation does not leave behind any non-empty variables,
// otherwise the nFactors and nEntries counts would be incorrect.
for(varid_t j=0; j<this->index_.size(); ++j)
for(Index j=0; j<this->index_.size(); ++j)
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
assert(this->operator[](j).empty());
#endif
index_.permute(permutation);
// storage_type original(this->index_.size());
// this->index_.swap(original);
// for(varid_t j=0; j<permutation.size(); ++j)
// for(Index j=0; j<permutation.size(); ++j)
// this->index_[j].swap(original[permutation[j]]);
}
@ -115,10 +115,10 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
// if block we assume at least one factor.
if(factorGraph.size() > 0) {
// Find highest-numbered variable
varid_t maxVar = 0;
Index maxVar = 0;
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
if(factor) {
BOOST_FOREACH(const varid_t key, factor->keys()) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
@ -132,8 +132,8 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
varid_t fvari = 0;
BOOST_FOREACH(const varid_t key, factorGraph[fi]->keys()) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
index_[key].push_back(mapped_factor_type(fi, fvari));
++ fvari;
++ nEntries_;
@ -149,12 +149,12 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
//void VariableIndex<Storage>::augment(const Derived& varIndex) {
// nFactors_ = std::max(nFactors_, varIndex.nFactors());
// nEntries_ = nEntries_ + varIndex.nEntries();
// varid_t originalSize = index_.size();
// Index originalSize = index_.size();
// index_.container().resize(std::max(index_.size(), varIndex.size()));
// index_.permutation().resize(index_.container().size());
// for(varid_t var=originalSize; var<index_.permutation().size(); ++var)
// for(Index var=originalSize; var<index_.permutation().size(); ++var)
// index_.permutation()[var] = var;
// for(varid_t var=0; var<varIndex.size(); ++var)
// for(Index var=0; var<varIndex.size(); ++var)
// index_[var].insert(index_[var].begin(), varIndex[var].begin(), varIndex[var].end());
//}
@ -166,10 +166,10 @@ void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) {
// if block we assume at least one factor.
if(factorGraph.size() > 0) {
// Find highest-numbered variable
varid_t maxVar = 0;
Index maxVar = 0;
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
if(factor) {
BOOST_FOREACH(const varid_t key, factor->keys()) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
@ -177,18 +177,18 @@ void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) {
}
// Allocate index
varid_t originalSize = index_.size();
Index originalSize = index_.size();
index_.container().resize(std::max(index_.size(), maxVar+1));
index_.permutation().resize(index_.container().size());
for(varid_t var=originalSize; var<index_.permutation().size(); ++var)
for(Index var=originalSize; var<index_.permutation().size(); ++var)
index_.permutation()[var] = var;
// Augment index mapping from variable id to factor index
size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
varid_t fvari = 0;
BOOST_FOREACH(const varid_t key, factorGraph[fi]->keys()) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
index_[key].push_back(mapped_factor_type(orignFactors + fi, fvari));
++ fvari;
++ nEntries_;
@ -225,7 +225,7 @@ bool VariableIndex<Storage>::equals(const Derived& other, double tol) const {
template<class Storage>
void VariableIndex<Storage>::print(const std::string& str) const {
std::cout << str;
varid_t var = 0;
Index var = 0;
BOOST_FOREACH(const mapped_type& variable, index_.container()) {
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
assert(rvar != index_.permutation().end());

8
inference/VariableSlots-inl.h
View File

@ -32,8 +32,8 @@ VariableSlots::VariableSlots(const FG& factorGraph) {
size_t jointFactorPos = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
assert(factor);
varid_t factorVarSlot = 0;
BOOST_FOREACH(const varid_t involvedVariable, *factor) {
Index factorVarSlot = 0;
BOOST_FOREACH(const Index involvedVariable, *factor) {
// Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it
// that we'll fill with the slot indices for each factor that
@ -41,9 +41,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) {
// the array entry for each factor that will indicate the factor
// does not involve the variable.
iterator thisVarSlots; bool inserted;
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, vector<varid_t>()));
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, vector<Index>()));
if(inserted)
thisVarSlots->second.resize(factorGraph.size(), numeric_limits<varid_t>::max());
thisVarSlots->second.resize(factorGraph.size(), numeric_limits<Index>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << endl;
++ factorVarSlot;

2
inference/VariableSlots.cpp
View File

@ -27,7 +27,7 @@ void VariableSlots::print(const std::string& str) const {
for(size_t i=0; i<this->begin()->second.size(); ++i) {
cout << " \t";
BOOST_FOREACH(const value_type& slot, *this) {
if(slot.second[i] == numeric_limits<varid_t>::max())
if(slot.second[i] == numeric_limits<Index>::max())
cout << "x" << "\t";
else
cout << slot.second[i] << "\t";

6
inference/VariableSlots.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
* interleaved.
*
* VariableSlots describes the 2D block structure of the combined factor. It
* is essentially a map<varid_t, vector<size_t> >. The varid_t is the real
* is essentially a map<Index, vector<size_t> >. The Index is the real
* variable index of the combined factor slot. The vector<size_t> tells, for
* each row-block (factor), which column-block (variable slot) from the
* component factor appears in this block of the combined factor.
@ -41,8 +41,8 @@ namespace gtsam {
*/
// Internal-use-only typedef for the VariableSlots map base class because this is such a long type name
typedef std::map<varid_t, std::vector<varid_t>, std::less<varid_t>,
boost::fast_pool_allocator<std::pair<const varid_t, std::vector<varid_t> > > > _VariableSlots_map;
typedef std::map<Index, std::vector<Index>, std::less<Index>,
boost::fast_pool_allocator<std::pair<const Index, std::vector<Index> > > > _VariableSlots_map;
class VariableSlots : public _VariableSlots_map, public Testable<VariableSlots> {

38
inference/inference-inl.h
View File

@ -48,7 +48,7 @@ BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph
// Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index.
for(varid_t var = 0; var < variableIndex.size(); ++var) {
for(Index var = 0; var < variableIndex.size(); ++var) {
Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional);
@ -68,7 +68,7 @@ Inference::Eliminate(FactorGraph& factorGraph, typename FactorGraph::variableind
/* ************************************************************************* */
template<class FactorGraph>
inline typename FactorGraph::bayesnet_type::shared_ptr
Inference::EliminateUntil(const FactorGraph& factorGraph, varid_t bound) {
Inference::EliminateUntil(const FactorGraph& factorGraph, Index bound) {
// Create a copy of the factor graph to eliminate in-place
FactorGraph eliminationGraph(factorGraph);
@ -80,13 +80,13 @@ Inference::EliminateUntil(const FactorGraph& factorGraph, varid_t bound) {
/* ************************************************************************* */
template<class FactorGraph>
typename FactorGraph::bayesnet_type::shared_ptr
Inference::EliminateUntil(FactorGraph& factorGraph, varid_t bound, typename FactorGraph::variableindex_type& variableIndex) {
Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex) {
typename FactorGraph::bayesnet_type::shared_ptr bayesnet(new typename FactorGraph::bayesnet_type);
// Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index.
for(varid_t var = 0; var < bound; ++var) {
for(Index var = 0; var < bound; ++var) {
typename FactorGraph::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional);
@ -98,7 +98,7 @@ Inference::EliminateUntil(FactorGraph& factorGraph, varid_t bound, typename Fact
/* ************************************************************************* */
template<class FactorGraph>
typename FactorGraph::bayesnet_type::sharedConditional
Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, varid_t var) {
Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var) {
/* This function performs symbolic elimination of a variable, comprising
* combining involved factors (analogous to "assembly" in SPQR) followed by
@ -150,14 +150,14 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
// key has been added yet, but the positions stored in the variableIndex are
// from the unsorted positions and will be fixed later.
tic("EliminateOne: Find involved vars");
map<varid_t, size_t, std::less<varid_t>, boost::fast_pool_allocator<pair<const varid_t,size_t> > > involvedKeys; // Variable and original order as discovered
map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > > involvedKeys; // Variable and original order as discovered
BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(debug) cout << removedFactorI << " is involved" << endl;
// If the factor has not previously been removed
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable.
BOOST_FOREACH(varid_t involvedVariable, factorGraph[removedFactorI]->keys()) {
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
// Mark the new joint factor as involving each variable in the removed factor.
assert(!variableIndex[involvedVariable].empty());
if(variableIndex[involvedVariable].back().factorIndex != jointFactorIndex) {
@ -182,11 +182,11 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
if(debug) cout << "Sorted keys:";
tic("EliminateOne: Sort involved vars");
vector<size_t> varposPermutation(involvedKeys.size(), numeric_limits<size_t>::max());
vector<varid_t> sortedKeys(involvedKeys.size());
vector<Index> sortedKeys(involvedKeys.size());
{
size_t sortedVarpos = 0;
const map<varid_t, size_t, std::less<varid_t>, boost::fast_pool_allocator<pair<const varid_t,size_t> > >& involvedKeysC(involvedKeys);
for(map<varid_t, size_t, std::less<varid_t>, boost::fast_pool_allocator<pair<const varid_t,size_t> > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) {
const map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > >& involvedKeysC(involvedKeys);
for(map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) {
sortedKeys[sortedVarpos] = key_pos->first;
assert(varposPermutation[key_pos->second] == numeric_limits<size_t>::max());
varposPermutation[key_pos->second] = sortedVarpos;
@ -203,7 +203,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
// Fix the variable positions in the variableIndex
tic("EliminateOne: Fix varIndex");
for(size_t sortedPos=0; sortedPos<sortedKeys.size(); ++sortedPos) {
varid_t var = sortedKeys[sortedPos];
Index var = sortedKeys[sortedPos];
assert(!variableIndex[var].empty());
assert(variableIndex[var].back().factorIndex == jointFactorIndex);
assert(sortedPos == varposPermutation[variableIndex[var].back().variablePosition]);
@ -232,7 +232,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
// Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable.
BOOST_FOREACH(varid_t involvedVariable, factorGraph[removedFactorI]->keys()) {
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
// Mark the new joint factor as involving each variable in the removed factor
assert(!variableIndex[involvedVariable].empty());
assert(variableIndex[involvedVariable].back().factorIndex == jointFactorIndex);
@ -301,7 +301,7 @@ typename FactorGraph::bayesnet_type::shared_ptr Inference::Marginal(const Factor
// the variables we want.
typename FactorGraph::bayesnet_type::shared_ptr marginal(new typename FactorGraph::bayesnet_type());
typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
for(varid_t j=0; j<variables.size(); ++j, ++conditional) {
for(Index j=0; j<variables.size(); ++j, ++conditional) {
marginal->push_front(*conditional);
assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end());
}
@ -325,7 +325,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
p[0] = 0;
int count = 0;
for(varid_t var = 0; var < variableIndex.size(); ++var) {
for(Index var = 0; var < variableIndex.size(); ++var) {
const typename VariableIndexType::mapped_type& column(variableIndex[var]);
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(const typename VariableIndexType::mapped_factor_type& factor_pos, column) {
@ -341,7 +341,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
if(constrainLast.size() < variableIndex.size()) {
BOOST_FOREACH(varid_t var, constrainLast) {
BOOST_FOREACH(Index var, constrainLast) {
assert(var < nVars);
cmember[var] = 1;
}
@ -366,7 +366,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
// Convert elimination ordering in p to an ordering
Permutation::shared_ptr permutation(new Permutation(nVars));
for (varid_t j = 0; j < nVars; j++) {
for (Index j = 0; j < nVars; j++) {
permutation->operator[](j) = p[j];
if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl;
}
@ -381,7 +381,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
// /* eliminate one node from the factor graph */
// /* ************************************************************************* */
// template<class Factor,class Conditional>
// boost::shared_ptr<Conditional> eliminateOne(FactorGraph<Factor>& graph, varid_t key) {
// boost::shared_ptr<Conditional> eliminateOne(FactorGraph<Factor>& graph, Index key) {
//
// // combine the factors of all nodes connected to the variable to be eliminated
// // if no factors are connected to key, returns an empty factor
@ -409,7 +409,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
// {
// BayesNet<Conditional> bayesNet; // empty
//
// BOOST_FOREACH(varid_t key, ordering) {
// BOOST_FOREACH(Index key, ordering) {
// boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key);
// bayesNet.push_back(cg);
// }
@ -426,7 +426,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
//
// // Get the keys of all variables and remove all keys we want the marginal for
// Ordering ord = bn.ordering();
// BOOST_FOREACH(varid_t key, keys) ord.remove(key); // TODO: O(n*k), faster possible?
// BOOST_FOREACH(Index key, keys) ord.remove(key); // TODO: O(n*k), faster possible?
//
// // eliminate partially,
// BayesNet<Conditional> conditional = eliminate<Factor,Conditional>(factorGraph,ord);

6
inference/inference.cpp
View File

@ -13,7 +13,7 @@ namespace gtsam {
/* ************************************************************************* */
Conditional::shared_ptr
Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, varid_t var) {
Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var) {
tic("EliminateOne");
@ -42,7 +42,7 @@ Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<
// key has been added yet, but the positions stored in the variableIndex are
// from the unsorted positions and will be fixed later.
tic("EliminateOne: Find involved vars");
typedef set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > InvolvedKeys;
typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > InvolvedKeys;
InvolvedKeys involvedKeys;
BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(debug) cout << removedFactorI << " is involved" << endl;
@ -50,7 +50,7 @@ Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable.
BOOST_FOREACH(varid_t involvedVariable, factorGraph[removedFactorI]->keys()) {
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
if(debug) cout << " pulls in variable " << involvedVariable << endl;
// Mark the new joint factor as involving each variable in the removed factor.
assert(!variableIndex[involvedVariable].empty());

12
inference/inference.h
View File

@ -58,7 +58,7 @@ class Conditional;
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr
EliminateUntil(const FactorGraph& factorGraph, varid_t bound);
EliminateUntil(const FactorGraph& factorGraph, Index bound);
/**
* Partially eliminate a factor graph, up to but not including the given
@ -67,7 +67,7 @@ class Conditional;
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr
EliminateUntil(FactorGraph& factorGraph, varid_t bound, typename FactorGraph::variableindex_type& variableIndex);
EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex);
/**
* Eliminate a single variable, updating an existing factor graph and
@ -75,7 +75,7 @@ class Conditional;
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::sharedConditional
EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, varid_t var);
EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var);
/**
* Eliminate a single variable, updating an existing factor graph and
@ -83,7 +83,7 @@ class Conditional;
* symbolic factor graphs.
*/
static boost::shared_ptr<Conditional>
EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, varid_t var);
EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var);
/**
* Eliminate all variables except the specified ones. Internally this
@ -100,7 +100,7 @@ class Conditional;
* Compute a permutation (variable ordering) using colamd
*/
template<class VariableIndexType>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<varid_t>()); }
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); }
template<class VariableIndexType, typename ConstraintContainer>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast);
@ -125,7 +125,7 @@ class Conditional;
// */
// template<class Factor, class Conditional>
// boost::shared_ptr<Conditional>
// eliminateOne(FactorGraph<Factor>& factorGraph, varid_t key);
// eliminateOne(FactorGraph<Factor>& factorGraph, Index key);
//
// /**
// * eliminate factor graph using the given (not necessarily complete)

22
inference/tests/testBayesTree.cpp
View File

@ -23,7 +23,7 @@ typedef BayesTree<Conditional> SymbolicBayesTree;
///* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper
static const varid_t _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
//Conditional::shared_ptr
// x3(new Conditional(_x3_)),
// x2(new Conditional(_x2_,_x3_)),
@ -46,7 +46,7 @@ static const varid_t _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
/* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence
static const varid_t _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
Conditional::shared_ptr
B(new Conditional(_B_)),
L(new Conditional(_L_, _B_)),
@ -102,13 +102,13 @@ TEST( BayesTree, constructor )
// Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_;
// IndexTable<Symbol> index(ordering);
list<varid_t> parents1; parents1 += _E_, _L_;
list<Index> parents1; parents1 += _E_, _L_;
CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1)));
list<varid_t> parents2; parents2 += _L_, _E_;
list<Index> parents2; parents2 += _L_, _E_;
CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2)));
list<varid_t> parents3; parents3 += _L_, _B_;
list<Index> parents3; parents3 += _L_, _B_;
CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3)));
}
@ -133,7 +133,7 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
/* ************************************************************************* */
TEST( BayesTree, removePath )
{
const varid_t _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
Conditional::shared_ptr
A(new Conditional(_A_)),
B(new Conditional(_B_, _A_)),
@ -236,7 +236,7 @@ TEST( BayesTree, removeTop )
// Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn;
SymbolicBayesTree::Cliques orphans;
list<varid_t> keys; keys += _B_,_S_;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
@ -277,7 +277,7 @@ TEST( BayesTree, removeTop2 )
// Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn;
SymbolicBayesTree::Cliques orphans;
list<varid_t> keys; keys += _B_,_S_;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
@ -296,7 +296,7 @@ TEST( BayesTree, removeTop2 )
/* ************************************************************************* */
TEST( BayesTree, removeTop3 )
{
const varid_t _x4_=5, _l5_=6;
const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated
Conditional::shared_ptr
X(new Conditional(_l5_)),
@ -313,7 +313,7 @@ TEST( BayesTree, removeTop3 )
bayesTree.insert(C);
// remove all
list<varid_t> keys;
list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<Conditional> bn;
SymbolicBayesTree::Cliques orphans;
@ -331,7 +331,7 @@ TEST( BayesTree, removeTop3 )
TEST( BayesTree, insert )
{
// construct bayes tree by split the graph along the separator x3 - x4
const varid_t _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
SymbolicFactorGraph fg1, fg2, fg3;
fg1.push_factor(_x3_, _x4_);
fg2.push_factor(_x1_, _x2_);

101
inference/tests/testEliminationTree.cpp
View File

@ -58,55 +58,76 @@ typedef EliminationTree<SymbolicFactorGraph> SymbolicEliminationTree;
// CHECK(assert_equal<SymbolicEliminationTree>(expected, actual));
//}
/* ************************************************************************* *
* graph: f(1,2) f(1,3) f(2,5) f(3,5) f(4,5)
* tree: x1 -> x2 -> x3 -> x5 <- x4 (arrow is parent pointer)
****************************************************************************/
//TEST( EliminationTree, constructor )
//{
// Index x1=1, x2=2, x3=3, x4=4, x5=5;
// SymbolicFactorGraph fc1,fc2,fc3,fc4,fc5;
//
// fc1.push_factor(x1,x2); fc1.push_factor(x1,x3);
// list<Index> c1sep; c1sep += x2,x3;
// SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end()));
//
// fc2.push_factor(x2,x5);
// list<Index> c2sep; c2sep += x3,x5;
// SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end()));
//
// fc3.push_factor(x3,x5);
// list<Index> c3sep; c3sep += x5;
// SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end()));
//
// fc4.push_factor(x4,x5);
// list<Index> c4sep; c4sep += x5;
// SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end()));
//
// list<Index> c5sep;
// SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end()));
//
// /** build expected tree using test accessor */
// SymbolicEliminationTree expected;
// _EliminationTreeTester<SymbolicFactorGraph> expected_(expected);
// expected_.nodes().resize(6);
// expected_.root() = c5; expected_.nodes()[x5]=c5;
// c5->addChild(c4); c4->parent()=c5; expected_.nodes()[x4]=c4;
// c5->addChild(c3); c3->parent()=c5; expected_.nodes()[x3]=c3;
// c3->addChild(c2); c2->parent()=c3; expected_.nodes()[x2]=c2;
// c2->addChild(c1); c1->parent()=c2; expected_.nodes()[x1]=c1;
//
// // build actual tree from factor graph (variant 2)
// SymbolicFactorGraph fg;
// fg.push_factor(x1,x2);
// fg.push_factor(x1,x3);
// fg.push_factor(x2,x5);
// fg.push_factor(x3,x5);
// fg.push_factor(x4,x5);
// SymbolicEliminationTree actual(fg);
//// GTSAM_PRINT(actual);
//
// CHECK(assert_equal<SymbolicEliminationTree>(expected, actual));
//}
/* ************************************************************************* *
* graph: f(1,2) f(1,3) f(2,5) f(3,5) f(4,5)
* tree: x1 -> x2 -> x3 -> x5 <- x4 (arrow is parent pointer)
****************************************************************************/
TEST( EliminationTree, constructor )
{
varid_t x1=1, x2=2, x3=3, x4=4, x5=5;
SymbolicFactorGraph fc1,fc2,fc3,fc4,fc5;
// Create factor graph
SymbolicFactorGraph fg;
fg.push_factor(1, 2);
fg.push_factor(1, 3);
fg.push_factor(2, 5);
fg.push_factor(3, 5);
fg.push_factor(4, 5);
fc1.push_factor(x1,x2); fc1.push_factor(x1,x3);
list<varid_t> c1sep; c1sep += x2,x3;
SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end()));
// Create elimination tree
SymbolicEliminationTree actual(fg);
fc2.push_factor(x2,x5);
list<varid_t> c2sep; c2sep += x3,x5;
SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end()));
// Check it
fc3.push_factor(x3,x5);
list<varid_t> c3sep; c3sep += x5;
SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end()));
fc4.push_factor(x4,x5);
list<varid_t> c4sep; c4sep += x5;
SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end()));
list<varid_t> c5sep;
SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end()));
/** build expected tree using test accessor */
SymbolicEliminationTree expected;
_EliminationTreeTester<SymbolicFactorGraph> expected_(expected);
expected_.nodes().resize(6);
expected_.root() = c5; expected_.nodes()[x5]=c5;
c5->addChild(c4); c4->parent()=c5; expected_.nodes()[x4]=c4;
c5->addChild(c3); c3->parent()=c5; expected_.nodes()[x3]=c3;
c3->addChild(c2); c2->parent()=c3; expected_.nodes()[x2]=c2;
c2->addChild(c1); c1->parent()=c2; expected_.nodes()[x1]=c1;
/** build actual tree from factor graph (variant 2) */
SymbolicFactorGraph fg;
fg.push_factor(x1,x2);
fg.push_factor(x1,x3);
fg.push_factor(x2,x5);
fg.push_factor(x3,x5);
fg.push_factor(x4,x5);
SymbolicEliminationTree actual(fg);
// GTSAM_PRINT(actual);
CHECK(assert_equal<SymbolicEliminationTree>(expected, actual));
}
/* ************************************************************************* */

12
inference/tests/testJunctionTree.cpp
View File

@ -33,7 +33,7 @@ typedef BayesTree<Conditional> SymbolicBayesTree;
****************************************************************************/
TEST( JunctionTree, constructor )
{
const varid_t x2=0, x1=1, x3=2, x4=3;
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);
@ -41,10 +41,10 @@ TEST( JunctionTree, constructor )
SymbolicJunctionTree actual(fg);
vector<varid_t> frontal1; frontal1 += x3, x4;
vector<varid_t> frontal2; frontal2 += x2, x1;
vector<varid_t> sep1;
vector<varid_t> sep2; sep2 += x3;
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));
LONGS_EQUAL(1, actual.root()->size());
@ -63,7 +63,7 @@ TEST( JunctionTree, constructor )
****************************************************************************/
TEST( JunctionTree, eliminate)
{
const varid_t x2=0, x1=1, x3=2, x4=3;
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraph fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);

8
inference/tests/testSymbolicBayesNet.cpp
View File

@ -17,10 +17,10 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
static const varid_t _L_ = 0;
static const varid_t _A_ = 1;
static const varid_t _B_ = 2;
static const varid_t _C_ = 3;
static const Index _L_ = 0;
static const Index _A_ = 1;
static const Index _B_ = 2;
static const Index _C_ = 3;
Conditional::shared_ptr
B(new Conditional(_B_)),

2
inference/tests/testSymbolicFactor.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
/* ************************************************************************* */
TEST(SymbolicFactor, eliminate) {
vector<varid_t> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
Factor actual(keys.begin(), keys.end());
BayesNet<Conditional> fragment = *actual.eliminate(3);

6
inference/tests/testSymbolicFactorGraph.cpp
View File

@ -18,9 +18,9 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
static const varid_t vx2=0;
static const varid_t vx1=1;
static const varid_t vl1=2;
static const Index vx2=0;
static const Index vx1=1;
static const Index vl1=2;
/* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminate2 )

28
linear/GaussianBayesNet.cpp
View File

@ -26,7 +26,7 @@ template class BayesNet<GaussianConditional>;
namespace gtsam {
/* ************************************************************************* */
GaussianBayesNet scalarGaussian(varid_t key, double mu, double sigma) {
GaussianBayesNet scalarGaussian(Index key, double mu, double sigma) {
GaussianBayesNet bn;
GaussianConditional::shared_ptr
conditional(new GaussianConditional(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1)));
@ -35,7 +35,7 @@ GaussianBayesNet scalarGaussian(varid_t key, double mu, double sigma) {
}
/* ************************************************************************* */
GaussianBayesNet simpleGaussian(varid_t key, const Vector& mu, double sigma) {
GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma) {
GaussianBayesNet bn;
size_t n = mu.size();
GaussianConditional::shared_ptr
@ -45,15 +45,15 @@ GaussianBayesNet simpleGaussian(varid_t key, const Vector& mu, double sigma) {
}
/* ************************************************************************* */
void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
varid_t name1, Matrix S, Vector sigmas) {
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Vector sigmas) {
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
bn.push_front(cg);
}
/* ************************************************************************* */
void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
varid_t name1, Matrix S, varid_t name2, Matrix T, Vector sigmas) {
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) {
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas));
bn.push_front(cg);
}
@ -61,7 +61,7 @@ void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
/* ************************************************************************* */
boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
vector<size_t> dimensions(bn.size());
varid_t var = 0;
Index var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->get_R().size1();
}
@ -102,7 +102,7 @@ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
// i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
varid_t i = cg->key();
Index i = cg->key();
Vector zi = emul(y[i],cg->get_sigmas());
GaussianConditional::const_iterator it;
for (it = cg->beginParents(); it!= cg->endParents(); it++) {
@ -126,18 +126,18 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
varid_t j = cg->key();
Index j = cg->key();
gy[j] = gtsam::backSubstituteUpper(gy[j],cg->get_R());
GaussianConditional::const_iterator it;
for (it = cg->beginParents(); it!= cg->endParents(); it++) {
const varid_t i = *it;
const Index i = *it;
transposeMultiplyAdd(-1.0,cg->get_S(it),gy[j],gy[i]);
}
}
// Scale gy
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
varid_t j = cg->key();
Index j = cg->key();
gy[j] = emul(gy[j],cg->get_sigmas());
}
@ -149,7 +149,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// add the dimensions of all variables to get matrix dimension
// and at the same time create a mapping from keys to indices
size_t N=0; map<varid_t,size_t> mapping;
size_t N=0; map<Index,size_t> mapping;
BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
mapping.insert(make_pair(cg->key(),N));
N += cg->dim();
@ -158,7 +158,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// create matrix and copy in values
Matrix R = zeros(N,N);
Vector d(N);
varid_t key; size_t I;
Index key; size_t I;
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
boost::shared_ptr<const GaussianConditional> cg = bn[key];
@ -198,7 +198,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
VectorValues rhs(const GaussianBayesNet& bn) {
boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn) {
varid_t key = cg->key();
Index key = cg->key();
// get sigmas
const Vector& sigmas = cg->get_sigmas();

12
linear/GaussianBayesNet.h
View File

@ -23,24 +23,24 @@ namespace gtsam {
typedef BayesNet<GaussianConditional> GaussianBayesNet;
/** Create a scalar Gaussian */
GaussianBayesNet scalarGaussian(varid_t key, double mu=0.0, double sigma=1.0);
GaussianBayesNet scalarGaussian(Index key, double mu=0.0, double sigma=1.0);
/** Create a simple Gaussian on a single multivariate variable */
GaussianBayesNet simpleGaussian(varid_t key, const Vector& mu, double sigma=1.0);
GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma=1.0);
/**
* Add a conditional node with one parent
* |Rx+Sy-d|
*/
void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
varid_t name1, Matrix S, Vector sigmas);
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Vector sigmas);
/**
* Add a conditional node with two parents
* |Rx+Sy+Tz-d|
*/
void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
varid_t name1, Matrix S, varid_t name2, Matrix T, Vector sigmas);
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas);
/**
* Allocate a VectorValues for the variables in a BayesNet

18
linear/GaussianConditional.cpp
View File

@ -22,10 +22,10 @@ namespace gtsam {
GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(varid_t key) : Conditional(key), rsd_(matrix_) {}
GaussianConditional::GaussianConditional(Index key) : Conditional(key), rsd_(matrix_) {}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(varid_t key,const Vector& d, const Matrix& R, const Vector& sigmas) :
GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) :
Conditional(key), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), 1 };
@ -35,8 +35,8 @@ GaussianConditional::GaussianConditional(varid_t key,const Vector& d, const Matr
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Matrix& R,
varid_t name1, const Matrix& S, const Vector& sigmas) :
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const Vector& sigmas) :
Conditional(key,name1), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), 1 };
@ -47,8 +47,8 @@ GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Mat
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Matrix& R,
varid_t name1, const Matrix& S, varid_t name2, const Matrix& T, const Vector& sigmas) :
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) :
Conditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 };
@ -60,7 +60,7 @@ GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Mat
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Matrix& R, const list<pair<varid_t, Matrix> >& parents, const Vector& sigmas) :
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
Conditional::nFrontal_ = 1;
@ -69,7 +69,7 @@ GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Mat
dims[0] = R.size2();
Conditional::factor_.keys_[0] = key;
size_t j=1;
for(std::list<std::pair<varid_t, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
Conditional::factor_.keys_[j] = parent->first;
dims[j] = parent->second.size2();
++ j;
@ -78,7 +78,7 @@ GaussianConditional::GaussianConditional(varid_t key, const Vector& d, const Mat
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size()));
ublas::noalias(rsd_(0)) = ublas::triangular_adaptor<const Matrix, ublas::upper>(R);
j = 1;
for(std::list<std::pair<varid_t, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
ublas::noalias(rsd_(j)) = parent->second;
++ j;
}

16
linear/GaussianConditional.h
View File

@ -59,31 +59,31 @@ public:
GaussianConditional();
/** constructor */
GaussianConditional(varid_t key);
GaussianConditional(Index key);
/** constructor with no parents
* |Rx-d|
*/
GaussianConditional(varid_t key, const Vector& d, const Matrix& R, const Vector& sigmas);
GaussianConditional(Index key, const Vector& d, const Matrix& R, const Vector& sigmas);
/** constructor with only one parent
* |Rx+Sy-d|
*/
GaussianConditional(varid_t key, const Vector& d, const Matrix& R,
varid_t name1, const Matrix& S, const Vector& sigmas);
GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const Vector& sigmas);
/** constructor with two parents
* |Rx+Sy+Tz-d|
*/
GaussianConditional(varid_t key, const Vector& d, const Matrix& R,
varid_t name1, const Matrix& S, varid_t name2, const Matrix& T, const Vector& sigmas);
GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas);
/**
* constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d|
*/
GaussianConditional(varid_t key, const Vector& d,
const Matrix& R, const std::list<std::pair<varid_t, Matrix> >& parents, const Vector& sigmas);
GaussianConditional(Index key, const Vector& d,
const Matrix& R, const std::list<std::pair<Index, Matrix> >& parents, const Vector& sigmas);
/**
* Constructor when matrices are already stored in a combined matrix, allows

72
linear/GaussianFactor.cpp
View File

@ -61,7 +61,7 @@ GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.si
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1,
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) :
Factor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), 1};
@ -71,7 +71,7 @@ GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1,
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1, varid_t i2, const Matrix& A2,
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) :
Factor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), 1};
@ -82,8 +82,8 @@ GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1, varid_t i2, const M
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1, varid_t i2, const Matrix& A2,
varid_t i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
Factor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size()));
@ -94,7 +94,7 @@ GaussianFactor::GaussianFactor(varid_t i1, const Matrix& A1, varid_t i2, const M
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(const std::vector<std::pair<varid_t, Matrix> > &terms,
GaussianFactor::GaussianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
keys_.resize(terms.size());
@ -113,13 +113,13 @@ GaussianFactor::GaussianFactor(const std::vector<std::pair<varid_t, Matrix> > &t
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(const std::list<std::pair<varid_t, Matrix> > &terms,
GaussianFactor::GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
keys_.resize(terms.size());
size_t dims[terms.size()+1];
size_t j=0;
for(std::list<std::pair<varid_t, Matrix> >::const_iterator term=terms.begin(); term!=terms.end(); ++term) {
for(std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin(); term!=terms.end(); ++term) {
keys_[j] = term->first;
dims[j] = term->second.size2();
++ j;
@ -128,7 +128,7 @@ GaussianFactor::GaussianFactor(const std::list<std::pair<varid_t, Matrix> > &ter
firstNonzeroBlocks_.resize(b.size(), 0);
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+terms.size()+1, b.size()));
j = 0;
for(std::list<std::pair<varid_t, Matrix> >::const_iterator term=terms.begin(); term!=terms.end(); ++term) {
for(std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin(); term!=terms.end(); ++term) {
Ab_(j) = term->second;
++ j;
}
@ -201,7 +201,7 @@ void GaussianFactor::print(const string& s) const {
cout << s << "\n";
if (empty()) {
cout << " empty, keys: ";
BOOST_FOREACH(const varid_t key, keys_) { cout << key << " "; }
BOOST_FOREACH(const Index key, keys_) { cout << key << " "; }
cout << endl;
} else {
for(const_iterator key=begin(); key!=end(); ++key)
@ -212,7 +212,7 @@ void GaussianFactor::print(const string& s) const {
}
///* ************************************************************************* */
//size_t GaussianFactor::getDim(varid_t key) const {
//size_t GaussianFactor::getDim(Index key) const {
// const_iterator it = findA(key);
// if (it != end())
// return it->second.size2();
@ -242,7 +242,7 @@ bool GaussianFactor::equals(const GaussianFactor& f, double tol) const {
/* ************************************************************************* */
void GaussianFactor::permuteWithInverse(const Permutation& inversePermutation) {
this->permuted_.value = true;
BOOST_FOREACH(varid_t& key, keys_) { key = inversePermutation[key]; }
BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
// Since we're permuting the variables, ensure that entire rows from this
// factor are copied when Combine is called
BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; }
@ -274,12 +274,12 @@ double GaussianFactor::error(const VectorValues& c) const {
//Dimensions GaussianFactor::dimensions() const {
// Dimensions result;
// BOOST_FOREACH(const NamedMatrix& jA, As_)
// result.insert(std::pair<varid_t,size_t>(jA.first,jA.second.size2()));
// result.insert(std::pair<Index,size_t>(jA.first,jA.second.size2()));
// return result;
//}
//
///* ************************************************************************* */
//void GaussianFactor::tally_separator(varid_t key, set<varid_t>& separator) const {
//void GaussianFactor::tally_separator(Index key, set<Index>& separator) const {
// if(involves(key)) {
// BOOST_FOREACH(const NamedMatrix& jA, As_)
// if(jA.first != key) separator.insert(jA.first);
@ -336,7 +336,7 @@ pair<Matrix,Vector> GaussianFactor::matrix(bool weight) const {
Matrix GaussianFactor::matrix_augmented(bool weight) const {
// // get pointers to the matrices
// vector<const Matrix *> matrices;
// BOOST_FOREACH(varid_t j, ordering) {
// BOOST_FOREACH(Index j, ordering) {
// const Matrix& Aj = get_A(j);
// matrices.push_back(&Aj);
// }
@ -386,7 +386,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
//
// // iterate over all matrices from the factor f
// BOOST_FOREACH(const NamedMatrix& p, f->As_) {
// varid_t key = p.first;
// Index key = p.first;
// const Matrix& Aj = p.second;
//
// // find the corresponding matrix among As
@ -657,13 +657,13 @@ struct _RowSource {
};
/* Explicit instantiations for storage types */
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<varid_t>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<varid_t>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template<class Storage>
GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const vector<size_t>& factors,
const vector<varid_t>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
shared_ptr ret(boost::make_shared<GaussianFactor>());
static const bool verbose = false;
@ -691,7 +691,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
size_t n = 0;
{
size_t j=0;
BOOST_FOREACH(const varid_t& var, variables) {
BOOST_FOREACH(const Index& var, variables) {
if(debug) cout << "Have variable " << var << endl;
dims[j] = variableIndex.dim(var);
n += dims[j];
@ -726,7 +726,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
size_t factorRowI = 0;
assert(factor.firstNonzeroBlocks_.size() == factor.numberOfRows());
BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.firstNonzeroBlocks_) {
varid_t firstNonzeroVar;
Index firstNonzeroVar;
if(factor.permuted_.value == true)
firstNonzeroVar = *std::min_element(factor.keys_.begin(), factor.keys_.end());
else
@ -772,11 +772,11 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// Copy the row of A variable by variable, starting at the first nonzero
// variable.
std::vector<varid_t>::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos;
std::vector<Index>::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos;
std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
ret->firstNonzeroBlocks_[row] = *varposIt;
if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
std::vector<varid_t>::const_iterator keyitend = factor.keys_.end();
std::vector<Index>::const_iterator keyitend = factor.keys_.end();
while(keyit != keyitend) {
const size_t varpos = *varposIt;
// If the factor is permuted, the varpos's in the joint factor could be
@ -827,14 +827,14 @@ boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<Gaussia
size_t m = 0;
size_t n = 0;
{
varid_t jointVarpos = 0;
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
varid_t sourceFactorI = 0;
BOOST_FOREACH(const varid_t sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<varid_t>::max()) {
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const GaussianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifndef NDEBUG
@ -888,7 +888,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
for(size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
const GaussianFactor& sourceFactor(*factors[sourceFactorI]);
for(size_t sourceFactorRow = 0; sourceFactorRow < sourceFactor.numberOfRows(); ++sourceFactorRow) {
varid_t firstNonzeroVar;
Index firstNonzeroVar;
if(sourceFactor.permuted_.value)
firstNonzeroVar = *std::min_element(sourceFactor.begin(), sourceFactor.end());
else
@ -914,12 +914,12 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// Copy rows
tic("Combine 4: copy rows");
varid_t combinedSlot = 0;
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
for(size_t row = 0; row < m; ++row) {
const varid_t sourceSlot = varslot.second[rowSources[row].factorI];
const Index sourceSlot = varslot.second[rowSources[row].factorI];
ab_type::block_type combinedBlock(combined->Ab_(combinedSlot));
if(sourceSlot != numeric_limits<varid_t>::max()) {
if(sourceSlot != numeric_limits<Index>::max()) {
const GaussianFactor& source(*factors[rowSources[row].factorI]);
const size_t sourceRow = rowSources[row].factorRowI;
assert(!source.permuted_.value || source.firstNonzeroBlocks_[sourceRow] == 0);
@ -937,7 +937,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// Copy rhs (b), sigma, and firstNonzeroBlocks
tic("Combine 5: copy vectors");
varid_t firstNonzeroSlot = 0;
Index firstNonzeroSlot = 0;
for(size_t row = 0; row < m; ++row) {
const GaussianFactor& source(*factors[rowSources[row].factorI]);
const size_t sourceRow = rowSources[row].factorRowI;
@ -1021,7 +1021,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// conditional->add(*itFrontal2, sub(Ab, n0, n1, j, j+dim));
// j+=dim;
// }
// BOOST_FOREACH(varid_t cur_key, separators) {
// BOOST_FOREACH(Index cur_key, separators) {
// size_t dim = dimensions.at(cur_key);
// conditional->add(cur_key, sub(Ab, n0, n1, j, j+dim));
// j+=dim;
@ -1043,7 +1043,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// // extract the new factor
// GaussianFactor::shared_ptr factor(new GaussianFactor);
// size_t j = n1;
// BOOST_FOREACH(varid_t cur_key, separators) {
// BOOST_FOREACH(Index cur_key, separators) {
// size_t dim = dimensions.at(cur_key); // TODO avoid find !
// factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
// j+=dim;
@ -1066,7 +1066,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
///* ************************************************************************* */
//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
//GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
// varid_t frontal, const Ordering& separator,
// Index frontal, const Ordering& separator,
// const Dimensions& dimensions) {
// Ordering frontals; frontals += frontal;
// pair<GaussianBayesNet, shared_ptr> ret =
@ -1075,7 +1075,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
//}
///* ************************************************************************* */
//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
//GaussianFactor::eliminate(varid_t key) const
//GaussianFactor::eliminate(Index key) const
//{
// // if this factor does not involve key, we exit with empty CG and LF
// const_iterator it = findA(key);
@ -1089,7 +1089,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
// // create an internal ordering that eliminates key first
// Ordering ordering;
// ordering += key;
// BOOST_FOREACH(varid_t k, keys())
// BOOST_FOREACH(Index k, keys())
// if (k != key) ordering += k;
//
// // extract [A b] from the combined linear factor (ensure that x is leading)

20
linear/GaussianFactor.h
View File

@ -40,7 +40,7 @@ class GaussianFactorGraph;
template<class VariableIndexStorage=VariableIndexStorage_vector> class GaussianVariableIndex;
/** A map from key to dimension, useful in various contexts */
typedef std::map<varid_t, size_t> Dimensions;
typedef std::map<Index, size_t> Dimensions;
/**
* Base Class for a linear factor.
@ -75,24 +75,24 @@ public:
GaussianFactor(const Vector& b_in);
/** Construct unary factor */
GaussianFactor(varid_t i1, const Matrix& A1,
GaussianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model);
/** Construct binary factor */
GaussianFactor(varid_t i1, const Matrix& A1,
varid_t i2, const Matrix& A2,
GaussianFactor(Index i1, const Matrix& A1,
Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model);
/** Construct ternary factor */
GaussianFactor(varid_t i1, const Matrix& A1, varid_t i2,
const Matrix& A2, varid_t i3, const Matrix& A3,
GaussianFactor(Index i1, const Matrix& A1, Index i2,
const Matrix& A2, Index i3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model);
/** Construct an n-ary factor */
GaussianFactor(const std::vector<std::pair<varid_t, Matrix> > &terms,
GaussianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model);
GaussianFactor(const std::list<std::pair<varid_t, Matrix> > &terms,
GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model);
/** Construct from Conditional Gaussian */
@ -140,7 +140,7 @@ public:
template<class Storage>
static shared_ptr Combine(const GaussianFactorGraph& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<varid_t>& variables, const std::vector<std::vector<size_t> >& variablePositions);
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions);
/**
* Named constructor for combining a set of factors with pre-computed set of
@ -208,7 +208,7 @@ public:
GaussianConditional::shared_ptr eliminateFirst();
GaussianBayesNet::shared_ptr eliminate(varid_t nFrontals = 1);
GaussianBayesNet::shared_ptr eliminate(Index nFrontals = 1);
friend class GaussianFactorGraph;
friend class Inference;

34
linear/GaussianFactorGraph.cpp
View File

@ -35,9 +35,9 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
}
/* ************************************************************************* */
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> >
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> >
GaussianFactorGraph::keys() const {
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > keys;
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;
@ -125,9 +125,9 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
//}
///* ************************************************************************* */
//set<varid_t> GaussianFactorGraph::find_separator(varid_t key) const
//set<Index> GaussianFactorGraph::find_separator(Index key) const
//{
// set<varid_t> separator;
// set<Index> separator;
// BOOST_FOREACH(const sharedFactor& factor,factors_)
// factor->tally_separator(key,separator);
//
@ -148,7 +148,7 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
// }
//
// // find the number of columns
// BOOST_FOREACH(varid_t key, order) {
// BOOST_FOREACH(Index key, order) {
// n += dimensions.at(key);
// }
//
@ -165,7 +165,7 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
// BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) {
// // loop through ordering
// size_t cur_n = 0;
// BOOST_FOREACH(varid_t key, order) {
// BOOST_FOREACH(Index key, order) {
// // copy over matrix if it exists
// if (factor->involves(key)) {
// insertSub(Ab, factor->get_A(key), cur_m, cur_n);
@ -205,24 +205,24 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
///* ************************************************************************* */
//GaussianConditional::shared_ptr
//GaussianFactorGraph::eliminateOneMatrixJoin(varid_t key) {
//GaussianFactorGraph::eliminateOneMatrixJoin(Index key) {
// // find and remove all factors connected to key
// vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
//
// // Collect all dimensions as well as the set of separator keys
// set<varid_t> separator;
// set<Index> separator;
// Dimensions dimensions;
// BOOST_FOREACH(const sharedFactor& factor, factors) {
// Dimensions factor_dim = factor->dimensions();
// dimensions.insert(factor_dim.begin(), factor_dim.end());
// BOOST_FOREACH(varid_t k, factor->keys())
// BOOST_FOREACH(Index k, factor->keys())
// if (!k == key)
// separator.insert(k);
// }
//
// // add the keys to the rendering
// Ordering render; render += key;
// BOOST_FOREACH(varid_t k, separator)
// BOOST_FOREACH(Index k, separator)
// if (k != key) render += k;
//
// // combine the factors to get a noisemodel and a combined matrix
@ -253,8 +253,8 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
//
// // collect separator
// Ordering separator;
// set<varid_t> frontal_set(frontals.begin(), frontals.end());
// BOOST_FOREACH(varid_t key, this->keys()) {
// set<Index> frontal_set(frontals.begin(), frontals.end());
// BOOST_FOREACH(Index key, this->keys()) {
// if (frontal_set.find(key) == frontal_set.end())
// separator.push_back(key);
// }
@ -304,7 +304,7 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
//GaussianFactorGraph::eliminate_(const Ordering& ordering)
//{
// boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
// BOOST_FOREACH(varid_t key, ordering) {
// BOOST_FOREACH(Index key, ordering) {
// GaussianConditional::shared_ptr cg = eliminateOne(key);
// chordalBayesNet->push_back(cg);
// }
@ -345,7 +345,7 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
// Dimensions result;
// BOOST_FOREACH(const sharedFactor& factor,factors_) {
// Dimensions vs = factor->dimensions();
// varid_t key; size_t dim;
// Index key; size_t dim;
// FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim));
// }
// return result;
@ -359,7 +359,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
// for each of the variables, add a prior
for(varid_t var=0; var<variableIndex.size(); ++var) {
for(Index var=0; var<variableIndex.size(); ++var) {
const GaussianVariableIndex<>::mapped_factor_type& factor_pos(variableIndex[var].front());
const GaussianFactor& factor(*operator[](factor_pos.factorIndex));
size_t dim = factor.getDim(factor.begin() + factor_pos.variablePosition);
@ -407,7 +407,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
// VectorValues config;
// Vector::const_iterator itSrc = vs.begin();
// Vector::iterator itDst;
// BOOST_FOREACH(varid_t key, ordering){
// BOOST_FOREACH(Index key, ordering){
// int dim = dims.find(key)->second;
// Vector v(dim);
// for (itDst=v.begin(); itDst!=v.end(); itDst++, itSrc++)
@ -428,7 +428,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
// // Find the starting index and dimensions for all variables given the order
// size_t j = 1;
// Dimensions result;
// BOOST_FOREACH(varid_t key, ordering) {
// BOOST_FOREACH(Index key, ordering) {
// // associate key with first column index
// result.insert(make_pair(key,j));
// // find dimension for this key

28
linear/GaussianFactorGraph.h
View File

@ -46,28 +46,28 @@ namespace gtsam {
}
/** Add a unary factor */
inline void add(varid_t key1, const Matrix& A1,
inline void add(Index key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
}
/** Add a binary factor */
inline void add(varid_t key1, const Matrix& A1,
varid_t key2, const Matrix& A2,
inline void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model)));
}
/** Add a ternary factor */
inline void add(varid_t key1, const Matrix& A1,
varid_t key2, const Matrix& A2,
varid_t key3, const Matrix& A3,
inline void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2,
Index key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model)));
}
/** Add an n-ary factor */
inline void add(const std::vector<std::pair<varid_t, Matrix> > &terms,
inline void add(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(terms,b,model)));
}
@ -76,7 +76,7 @@ namespace gtsam {
* Return the set of variables involved in the factors (computes a set
* union).
*/
std::set<varid_t, std::less<varid_t>, boost::fast_pool_allocator<varid_t> > keys() const;
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys() const;
/** Permute the variables in the factors */
void permuteWithInverse(const Permutation& inversePermutation);
@ -121,13 +121,13 @@ namespace gtsam {
// * find the separator, i.e. all the nodes that have at least one
// * common factor with the given node. FD: not used AFAIK.
// */
// std::set<varid_t> find_separator(varid_t key) const;
// std::set<Index> find_separator(Index key) const;
// /**
// * Peforms a supposedly-faster (fewer matrix copy) version of elimination
// * CURRENTLY IN TESTING
// */
// GaussianConditional::shared_ptr eliminateOneMatrixJoin(varid_t key);
// GaussianConditional::shared_ptr eliminateOneMatrixJoin(Index key);
//
//
// /**
@ -297,7 +297,7 @@ namespace gtsam {
GaussianVariableIndex(const VariableIndex<VariableIndexStorage>& variableIndex, const storage_type& dimensions);
const storage_type& dims() const { return dims_; }
size_t dim(varid_t variable) const { Base::checkVar(variable); return dims_[variable]; }
size_t dim(Index variable) const { Base::checkVar(variable); return dims_[variable]; }
/** Permute */
void permute(const Permutation& permutation);
@ -336,7 +336,7 @@ namespace gtsam {
void GaussianVariableIndex<Storage>::fillDims(const GaussianFactorGraph& factorGraph) {
// Store dimensions of each variable
assert(dims_.size() == Base::index_.size());
for(varid_t var=0; var<Base::index_.size(); ++var)
for(Index var=0; var<Base::index_.size(); ++var)
if(!Base::index_[var].empty()) {
size_t factorIndex = Base::operator [](var).front().factorIndex;
size_t variablePosition = Base::operator [](var).front().variablePosition;
@ -352,7 +352,7 @@ namespace gtsam {
VariableIndex<Storage>::permute(permutation);
storage_type original(this->dims_.size());
this->dims_.swap(original);
for(varid_t j=0; j<permutation.size(); ++j)
for(Index j=0; j<permutation.size(); ++j)
this->dims_[j] = original[permutation[j]];
}
@ -371,7 +371,7 @@ namespace gtsam {
dims_[*var] = factor->getDim(var);
}
}
// for(varid_t var=0; var<dims_.size(); ++var) {
// for(Index var=0; var<dims_.size(); ++var) {
//#ifndef NDEBUG
// if(var >= varIndex.dims_.size() || varIndex.dims_[var] == 0)
// assert(dims_[var] != 0);

20
linear/VectorMap.cpp
View File

@ -18,7 +18,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void check_size(varid_t key, const Vector & vj, const Vector & dj) {
void check_size(Index key, const Vector & vj, const Vector & dj) {
if (dj.size()!=vj.size()) {
cout << "For key \"" << key << "\"" << endl;
cout << "vj.size = " << vj.size() << endl;
@ -28,21 +28,21 @@ void check_size(varid_t key, const Vector & vj, const Vector & dj) {
}
/* ************************************************************************* */
std::vector<varid_t> VectorMap::get_names() const {
std::vector<varid_t> names;
std::vector<Index> VectorMap::get_names() const {
std::vector<Index> names;
for(const_iterator it=values.begin(); it!=values.end(); it++)
names.push_back(it->first);
return names;
}
/* ************************************************************************* */
VectorMap& VectorMap::insert(varid_t name, const Vector& v) {
VectorMap& VectorMap::insert(Index name, const Vector& v) {
values.insert(std::make_pair(name,v));
return *this;
}
/* ************************************************************************* */
VectorMap& VectorMap::insertAdd(varid_t j, const Vector& a) {
VectorMap& VectorMap::insertAdd(Index j, const Vector& a) {
Vector& vj = values[j];
if (vj.size()==0) vj = a; else vj += a;
return *this;
@ -69,12 +69,12 @@ size_t VectorMap::dim() const {
}
/* ************************************************************************* */
const Vector& VectorMap::operator[](varid_t name) const {
const Vector& VectorMap::operator[](Index name) const {
return values.at(name);
}
/* ************************************************************************* */
Vector& VectorMap::operator[](varid_t name) {
Vector& VectorMap::operator[](Index name) {
return values.at(name);
}
@ -137,7 +137,7 @@ Vector VectorMap::vector() const {
Vector result(dim());
size_t cur_dim = 0;
varid_t j; Vector vj;
Index j; Vector vj;
FOREACH_PAIR(j, vj, values) {
subInsert(result, vj, cur_dim);
cur_dim += vj.size();
@ -149,7 +149,7 @@ Vector VectorMap::vector() const {
VectorMap expmap(const VectorMap& original, const VectorMap& delta)
{
VectorMap newValues;
varid_t j; Vector vj; // rtodo: copying vector?
Index j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) {
const Vector& dj = delta[j];
@ -167,7 +167,7 @@ VectorMap expmap(const VectorMap& original, const Vector& delta)
{
VectorMap newValues;
size_t i = 0;
varid_t j; Vector vj; // rtodo: copying vector?
Index j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj);

26
linear/VectorMap.h
View File

@ -23,29 +23,29 @@ namespace gtsam {
protected:
/** Map from string indices to values */
std::map<varid_t,Vector> values;
std::map<Index,Vector> values;
public:
typedef std::map<varid_t,Vector>::iterator iterator;
typedef std::map<varid_t,Vector>::const_iterator const_iterator;
typedef std::map<Index,Vector>::iterator iterator;
typedef std::map<Index,Vector>::const_iterator const_iterator;
VectorMap() {}
VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {}
VectorMap(varid_t j, const Vector& a) { insert(j,a); }
VectorMap(Index j, const Vector& a) { insert(j,a); }
virtual ~VectorMap() {}
/** return all the nodes in the graph **/
std::vector<varid_t> get_names() const;
std::vector<Index> get_names() const;
/** convert into a single large vector */
Vector vector() const;
/** Insert a value into the values structure with a given index */
VectorMap& insert(varid_t name, const Vector& v);
VectorMap& insert(Index name, const Vector& v);
/** Insert or add a value with given index */
VectorMap& insertAdd(varid_t j, const Vector& v);
VectorMap& insertAdd(Index j, const Vector& v);
/** Insert a config into another config */
void insert(const VectorMap& config);
@ -59,15 +59,15 @@ namespace gtsam {
iterator end() {return values.end();}
/** Vector access in VectorMap is via a Vector reference */
Vector& operator[](varid_t j);
const Vector& operator[](varid_t j) const;
Vector& operator[](Index j);
const Vector& operator[](Index j) const;
/** [set] and [get] provided for access via MATLAB */
inline Vector& get(varid_t j) { return (*this)[j];}
void set(varid_t j, const Vector& v) { (*this)[j] = v; }
inline const Vector& get(varid_t j) const { return (*this)[j];}
inline Vector& get(Index j) { return (*this)[j];}
void set(Index j, const Vector& v) { (*this)[j] = v; }
inline const Vector& get(Index j) const { return (*this)[j];}
bool contains(varid_t name) const {
bool contains(Index name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}

38
linear/VectorValues.h
View File

@ -52,7 +52,7 @@ public:
VectorValues(const Container& dimensions);
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(varid_t nVars, size_t varDim);
VectorValues(Index nVars, size_t varDim);
/** Construct from a container of variable dimensions in variable order and
* a combined Vector of all of the variables in order.
@ -60,11 +60,11 @@ public:
VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
/** Element access */
mapped_type operator[](varid_t variable);
const_mapped_type operator[](varid_t variable) const;
mapped_type operator[](Index variable);
const_mapped_type operator[](Index variable) const;
/** Number of elements */
varid_t size() const { return varStarts_.size()-1; }
Index size() const { return varStarts_.size()-1; }
/** Total dimensionality used (could be smaller than what has been allocated
* with reserve(...) ).
@ -81,14 +81,14 @@ public:
const_iterator end() const { return _impl_iterator<const VectorValues>(*this, varStarts_.size()-1); }
/** Reserve space for a total number of variables and dimensionality */
void reserve(varid_t nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); }
void reserve(Index nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); }
/**
* Append a variable using the next variable ID, and return that ID. Space
* must have been allocated ahead of time using reserve(...).
*/
varid_t push_back_preallocated(const Vector& vector) {
varid_t var = varStarts_.size()-1;
Index push_back_preallocated(const Vector& vector) {
Index var = varStarts_.size()-1;
varStarts_.push_back(varStarts_.back()+vector.size());
this->operator[](var) = vector; // This will assert that values_ has enough allocated space.
return var;
@ -100,7 +100,7 @@ public:
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const {
std::cout << str << ": " << varStarts_.size()-1 << " elements\n";
for(varid_t var=0; var<size(); ++var) {
for(Index var=0; var<size(); ++var) {
std::cout << " " << var << " " << operator[](var) << "\n";
}
std::cout.flush();
@ -110,7 +110,7 @@ public:
bool equals(const VectorValues& expected, double tol=1e-9) const {
if(size() != expected.size()) return false;
// iterate over all elements
for(varid_t var=0; var<size(); ++var)
for(Index var=0; var<size(); ++var)
if(!equal_with_abs_tol(expected[var],operator[](var),tol))
return false;
return true;
@ -136,9 +136,9 @@ public:
class _impl_iterator {
protected:
C& config_;
varid_t curVariable_;
Index curVariable_;
_impl_iterator(C& config, varid_t curVariable) : config_(config), curVariable_(curVariable) {}
_impl_iterator(C& config, Index curVariable) : config_(config), curVariable_(curVariable) {}
void checkCompat(const _impl_iterator<C>& r) { assert(&config_ == &r.config_); }
friend class VectorValues;
@ -157,14 +157,14 @@ public:
};
protected:
void checkVariable(varid_t variable) const { assert(variable < varStarts_.size()-1); }
void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); }
};
//inline VectorValues::VectorValues(const GaussianVariableIndex& variableIndex) : varStarts_(variableIndex.size()+1) {
// size_t varStart = 0;
// varStarts_[0] = 0;
// for(varid_t var=0; var<variableIndex.size(); ++var) {
// for(Index var=0; var<variableIndex.size(); ++var) {
// varStart += variableIndex.dim(var);
// varStarts_[var+1] = varStart;
// }
@ -175,17 +175,17 @@ template<class Container>
inline VectorValues::VectorValues(const Container& dimensions) : varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
varid_t var = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
values_.resize(varStarts_.back(), false);
}
inline VectorValues::VectorValues(varid_t nVars, size_t varDim) : varStarts_(nVars+1) {
inline VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars+1) {
varStarts_[0] = 0;
size_t varStart = 0;
for(varid_t j=1; j<=nVars; ++j)
for(Index j=1; j<=nVars; ++j)
varStarts_[j] = (varStart += varDim);
values_.resize(varStarts_.back(), false);
}
@ -194,20 +194,20 @@ inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const V
values_(values), varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
varid_t var = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
assert(varStarts_.back() == values.size());
}
inline VectorValues::mapped_type VectorValues::operator[](varid_t variable) {
inline VectorValues::mapped_type VectorValues::operator[](Index variable) {
checkVariable(variable);
return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
}
inline VectorValues::const_mapped_type VectorValues::operator[](varid_t variable) const {
inline VectorValues::const_mapped_type VectorValues::operator[](Index variable) const {
checkVariable(variable);
return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));

12
linear/tests/testGaussianConditional.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
using namespace std;
using namespace boost::assign;
static const varid_t _x_=0, _x1_=1, _l1_=2;
static const Index _x_=0, _x1_=1, _l1_=2;
/* ************************************************************************* */
TEST(GaussianConditional, constructor)
@ -47,7 +47,7 @@ TEST(GaussianConditional, constructor)
Vector d = Vector_(2, 1.0, 2.0);
Vector s = Vector_(2, 3.0, 4.0);
list<pair<varid_t, Matrix> > terms;
list<pair<Index, Matrix> > terms;
terms +=
make_pair(3, S1),
make_pair(5, S2),
@ -56,21 +56,21 @@ TEST(GaussianConditional, constructor)
GaussianConditional actual(1, d, R, terms, s);
GaussianConditional::const_iterator it = actual.beginFrontals();
CHECK(assert_equal(varid_t(1), *it));
CHECK(assert_equal(Index(1), *it));
CHECK(assert_equal(R, actual.get_R()));
++ it;
CHECK(it == actual.endFrontals());
it = actual.beginParents();
CHECK(assert_equal(varid_t(3), *it));
CHECK(assert_equal(Index(3), *it));
CHECK(assert_equal(S1, actual.get_S(it)));
++ it;
CHECK(assert_equal(varid_t(5), *it));
CHECK(assert_equal(Index(5), *it));
CHECK(assert_equal(S2, actual.get_S(it)));
++ it;
CHECK(assert_equal(varid_t(7), *it));
CHECK(assert_equal(Index(7), *it));
CHECK(assert_equal(S3, actual.get_S(it)));
++it;

36
linear/tests/testGaussianFactor.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
using namespace boost;
namespace ublas = boost::numeric::ublas;
static const varid_t _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
@ -41,7 +41,7 @@ TEST( GaussianFactor, constructor)
{
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
std::list<std::pair<varid_t, Matrix> > terms;
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
GaussianFactor actual(terms, b, noise);
@ -54,7 +54,7 @@ TEST( GaussianFactor, constructor2)
{
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
std::list<std::pair<varid_t, Matrix> > terms;
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
GaussianFactor actual(terms, b, noise);
@ -253,7 +253,7 @@ TEST(GaussianFactor, Combine2)
// exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
// exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
//
// vector<pair<varid_t, Matrix> > meas;
// vector<pair<Index, Matrix> > meas;
// meas.push_back(make_pair(_x1_, A22));
// GaussianFactor expected(meas, exb, sigmas);
// CHECK(assert_equal(expected,combined));
@ -275,7 +275,7 @@ TEST(GaussianFactor, Combine2)
//
// GaussianFactor combinedFactor(f);
//
// vector<pair<varid_t, Matrix> > combinedMeasurement;
// vector<pair<Index, Matrix> > combinedMeasurement;
// combinedMeasurement.push_back(make_pair(_x1_, Matrix_(8,2,
// 1.0, 0.0,
// 0.0, 1.0,
@ -352,7 +352,7 @@ TEST( GaussianFactor, eliminate2 )
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<varid_t, Matrix> > meas;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(_x2_, Ax2));
meas.push_back(make_pair(_l11_, Al1x1));
GaussianFactor combined(meas, b2, sigmas);
@ -407,7 +407,7 @@ TEST(GaussianFactor, eliminateFrontals)
0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.);
list<pair<varid_t, Matrix> > terms1;
list<pair<Index, Matrix> > terms1;
terms1 +=
make_pair(3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))),
make_pair(5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))),
@ -417,7 +417,7 @@ TEST(GaussianFactor, eliminateFrontals)
Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4));
GaussianFactor::shared_ptr factor1(new GaussianFactor(terms1, b1, sharedSigma(4, 0.5)));
list<pair<varid_t, Matrix> > terms2;
list<pair<Index, Matrix> > terms2;
terms2 +=
make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))),
make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))),
@ -426,7 +426,7 @@ TEST(GaussianFactor, eliminateFrontals)
Vector b2 = ublas::project(ublas::column(Ab, 10), ublas::range(4,8));
GaussianFactor::shared_ptr factor2(new GaussianFactor(terms2, b2, sharedSigma(4, 0.5)));
list<pair<varid_t, Matrix> > terms3;
list<pair<Index, Matrix> > terms3;
terms3 +=
make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))),
@ -434,7 +434,7 @@ TEST(GaussianFactor, eliminateFrontals)
Vector b3 = ublas::project(ublas::column(Ab, 10), ublas::range(8,12));
GaussianFactor::shared_ptr factor3(new GaussianFactor(terms3, b3, sharedSigma(4, 0.5)));
list<pair<varid_t, Matrix> > terms4;
list<pair<Index, Matrix> > terms4;
terms4 +=
make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10)));
Vector b4 = ublas::project(ublas::column(Ab, 10), ublas::range(12,14));
@ -464,7 +464,7 @@ TEST(GaussianFactor, eliminateFrontals)
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635);
Matrix R1 = ublas::project(R, ublas::range(0,2), ublas::range(0,2));
list<pair<varid_t, Matrix> > cterms1;
list<pair<Index, Matrix> > cterms1;
cterms1 +=
make_pair(5, ublas::project(R, ublas::range(0,2), ublas::range(2,4))),
make_pair(7, ublas::project(R, ublas::range(0,2), ublas::range(4,6))),
@ -474,7 +474,7 @@ TEST(GaussianFactor, eliminateFrontals)
GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2)));
Matrix R2 = ublas::project(R, ublas::range(2,4), ublas::range(2,4));
list<pair<varid_t, Matrix> > cterms2;
list<pair<Index, Matrix> > cterms2;
cterms2 +=
make_pair(7, ublas::project(R, ublas::range(2,4), ublas::range(4,6))),
make_pair(9, ublas::project(R, ublas::range(2,4), ublas::range(6,8))),
@ -483,7 +483,7 @@ TEST(GaussianFactor, eliminateFrontals)
GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2)));
Matrix R3 = ublas::project(R, ublas::range(4,6), ublas::range(4,6));
list<pair<varid_t, Matrix> > cterms3;
list<pair<Index, Matrix> > cterms3;
cterms3 +=
make_pair(9, ublas::project(R, ublas::range(4,6), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(4,6), ublas::range(8,10)));
@ -501,8 +501,8 @@ TEST(GaussianFactor, eliminateFrontals)
CHECK(assert_equal(expectedFragment, actualFragment, 0.001));
CHECK(assert_equal(size_t(2), actualFactor.keys().size()));
CHECK(assert_equal(varid_t(9), actualFactor.keys()[0]));
CHECK(assert_equal(varid_t(11), actualFactor.keys()[1]));
CHECK(assert_equal(Index(9), actualFactor.keys()[0]));
CHECK(assert_equal(Index(11), actualFactor.keys()[1]));
CHECK(assert_equal(Ae1, actualFactor.getA(actualFactor.begin()), 0.001));
CHECK(assert_equal(Ae2, actualFactor.getA(actualFactor.begin()+1), 0.001));
CHECK(assert_equal(be, actualFactor.getb(), 0.001));
@ -562,7 +562,7 @@ void print(const list<T>& i) {
//{
// GaussianFactor f(_x1_, eye(2), _x2_, eye(2), _l1_, eye(2), ones(2), sigma0_1);
//
// std::set<varid_t> act1, act2, act3;
// std::set<Index> act1, act2, act3;
// f.tally_separator(_x1_, act1);
// f.tally_separator(_x2_, act2);
// f.tally_separator(_l1_, act3);
@ -621,7 +621,7 @@ TEST ( GaussianFactor, constraint_eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
varid_t key = _x0_;
Index key = _x0_;
GaussianFactor lc(key, eye(2), v, constraintModel);
// eliminate it
@ -683,7 +683,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
//{
// Vector b = Vector_(3, 1., 2., 3.);
// SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
// std::list<std::pair<varid_t, Matrix> > terms;
// std::list<std::pair<Index, Matrix> > terms;
// terms.push_back(make_pair(_x0_, eye(2)));
// terms.push_back(make_pair(_x1_, 2.*eye(2)));
//

2
linear/tests/testGaussianJunctionTree.cpp
View File

@ -19,7 +19,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
static const varid_t x2=0, x1=1, x3=2, x4=3;
static const Index x2=0, x1=1, x3=2, x4=3;
GaussianFactorGraph createChain() {

4
linear/tests/testVectorMap.cpp
View File

@ -24,8 +24,8 @@
using namespace std;
using namespace gtsam;
static const varid_t l1=0, x1=1, x2=2;
static const varid_t _a_=3, _x_=4, _y_=5, _g_=6, _p_=7;
static const Index l1=0, x1=1, x2=2;
static const Index _a_=3, _x_=4, _y_=5, _g_=6, _p_=7;
/* ************************************************************************* */
VectorMap smallVectorMap() {

2
linear/tests/timeFactorOverhead.cpp
View File

@ -20,7 +20,7 @@ static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost
int main(int argc, char *argv[]) {
varid_t key = 0;
Index key = 0;
size_t vardim = 2;
size_t blockdim = 1;

2
linear/tests/timeGaussianFactor.cpp
View File

@ -22,7 +22,7 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
static const varid_t _x1_=1, _x2_=2, _l1_=3;
static const Index _x1_=1, _x2_=2, _l1_=3;
/*
* Alex's Machine

8
linear/tests/timeSLAMlike.cpp
View File

@ -21,7 +21,7 @@ using namespace boost::lambda;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
bool _pair_compare(const pair<varid_t, Matrix>& a1, const pair<varid_t, Matrix>& a2) { return a1.first < a2.first; }
bool _pair_compare(const pair<Index, Matrix>& a1, const pair<Index, Matrix>& a2) { return a1.first < a2.first; }
int main(int argc, char *argv[]) {
@ -58,14 +58,14 @@ int main(int argc, char *argv[]) {
SharedDiagonal noise = sharedSigma(blockdim, 1.0);
for(size_t c=0; c<nVars; ++c) {
for(size_t d=0; d<blocksPerVar; ++d) {
vector<pair<varid_t, Matrix> > terms; terms.reserve(varsPerBlock);
vector<pair<Index, Matrix> > terms; terms.reserve(varsPerBlock);
if(c == 0 && d == 0)
// If it's the first factor, just make a prior
terms.push_back(make_pair(0, eye(vardim)));
else if(c != 0) {
// Generate a random Gaussian factor
for(size_t h=0; h<varsPerBlock; ++h) {
varid_t var;
Index var;
// If it's the first factor for this variable, make it "odometry"
if(d == 0 && h == 0)
var = c-1;
@ -76,7 +76,7 @@ int main(int argc, char *argv[]) {
do
var = c + ri();
while(var < 0 || var > nVars-1 || find_if(terms.begin(), terms.end(),
boost::bind(&pair<varid_t, Matrix>::first, boost::lambda::_1) == var) != terms.end());
boost::bind(&pair<Index, Matrix>::first, boost::lambda::_1) == var) != terms.end());
Matrix A(blockdim, vardim);
for(size_t j=0; j<blockdim; ++j)
for(size_t k=0; k<vardim; ++k)

2
nonlinear/LieValues-inl.h
View File

@ -153,7 +153,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
Ordering::shared_ptr LieValues<J>::orderingArbitrary(varid_t firstVar) const {
Ordering::shared_ptr LieValues<J>::orderingArbitrary(Index firstVar) const {
// Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);

8
nonlinear/LieValues.h
View File

@ -181,7 +181,7 @@ namespace gtsam {
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(varid_t firstVar = 0) const;
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
private:
/** Serialization function */
@ -198,7 +198,7 @@ namespace gtsam {
std::vector<size_t> dimensions;
_ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> void operator()(const I& key_value) {
varid_t var = ordering[key_value->first];
Index var = ordering[key_value->first];
assert(var < dimensions.size());
dimensions[var] = key_value->second.dim();
}
@ -206,9 +206,9 @@ namespace gtsam {
/* ************************************************************************* */
struct _ValuesKeyOrderer {
varid_t var;
Index var;
Ordering::shared_ptr ordering;
_ValuesKeyOrderer(varid_t firstVar) : var(firstVar), ordering(new Ordering) {}
_ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {}
template<typename I> void operator()(const I& key_value) {
ordering->insert(key_value->first, var);
++ var;

10
nonlinear/NonlinearConstraint.h
View File

@ -155,7 +155,7 @@ public:
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
varid_t var = ordering[key_];
Index var = ordering[key_];
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
return GaussianFactor::shared_ptr(new GaussianFactor(var, A, b, model));
}
@ -267,7 +267,7 @@ public:
Matrix grad1, grad2;
Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
varid_t var1 = ordering[j1], var2 = ordering[j2];
Index var1 = ordering[j1], var2 = ordering[j2];
if (var1 < var2)
GaussianFactor::shared_ptr(new GaussianFactor(var1, grad1, var2, grad2, g, model));
else
@ -284,7 +284,7 @@ public:
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const {
const varid_t var1 = ordering[key1_], var2 = ordering[key2_];
const Index var1 = ordering[key1_], var2 = ordering[key2_];
if(var1 < var2)
return Factor::shared_ptr(new Factor(var1, var2));
else
@ -396,7 +396,7 @@ public:
Matrix A1, A2, A3;
Vector b = -1.0 * evaluateError(x1, x2, x3, A1, A2, A3);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
varid_t var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3];
Index var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3];
// perform sorting
if(var1 < var2 && var2 < var3)
@ -430,7 +430,7 @@ public:
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const {
const varid_t var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
if(var1 < var2 && var2 < var3)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_]));
else if(var2 < var1 && var1 < var3)

10
nonlinear/NonlinearFactor.h
View File

@ -214,7 +214,7 @@ namespace gtsam {
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
varid_t var = ordering[key_];
Index var = ordering[key_];
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
@ -328,7 +328,7 @@ namespace gtsam {
const X2& x2 = c[key2_];
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
const varid_t var1 = ordering[key1_], var2 = ordering[key2_];
const Index var1 = ordering[key1_], var2 = ordering[key2_];
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
@ -352,7 +352,7 @@ namespace gtsam {
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const {
const varid_t var1 = ordering[key1_], var2 = ordering[key2_];
const Index var1 = ordering[key1_], var2 = ordering[key2_];
if(var1 < var2)
return Factor::shared_ptr(new Factor(var1, var2));
else
@ -470,7 +470,7 @@ namespace gtsam {
const X3& x3 = c[key3_];
Matrix A1, A2, A3;
Vector b = -evaluateError(x1, x2, x3, A1, A2, A3);
const varid_t var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
@ -509,7 +509,7 @@ namespace gtsam {
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const {
const varid_t var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
if(var1 < var2 && var2 < var3)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_]));
else if(var2 < var1 && var1 < var3)

2
nonlinear/Ordering.cpp
View File

@ -41,7 +41,7 @@ bool Ordering::equals(const Ordering& rhs, double tol) const {
/* ************************************************************************* */
void Unordered::print(const string& s) const {
cout << s << " (" << size() << "):";
BOOST_FOREACH(varid_t key, *this)
BOOST_FOREACH(Index key, *this)
cout << " " << key;
cout << endl;
}

34
nonlinear/Ordering.h
View File

@ -21,51 +21,51 @@ namespace gtsam {
class Ordering : Testable<Ordering> {
protected:
typedef boost::fast_pool_allocator<std::pair<const Symbol, varid_t> > Allocator;
typedef std::map<Symbol, varid_t, std::less<Symbol>, Allocator> Map;
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
Map order_;
varid_t nVars_;
Index nVars_;
public:
typedef boost::shared_ptr<Ordering> shared_ptr;
typedef std::pair<const Symbol, varid_t> value_type;
typedef std::pair<const Symbol, Index> value_type;
typedef Map::iterator iterator;
typedef Map::const_iterator const_iterator;
Ordering() : nVars_(0) {}
/** One greater than the maximum ordering index. */
varid_t nVars() const { return nVars_; }
Index nVars() const { return nVars_; }
/** The number of variables in this ordering. */
varid_t size() const { return order_.size(); }
Index size() const { return order_.size(); }
iterator begin() { return order_.begin(); }
const_iterator begin() const { return order_.begin(); }
iterator end() { return order_.end(); }
const_iterator end() const { return order_.end(); }
varid_t& at(const Symbol& key) { return operator[](key); }
varid_t at(const Symbol& key) const { return operator[](key); }
varid_t& operator[](const Symbol& key) {
Index& at(const Symbol& key) { return operator[](key); }
Index at(const Symbol& key) const { return operator[](key); }
Index& operator[](const Symbol& key) {
iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
varid_t operator[](const Symbol& key) const {
Index operator[](const Symbol& key) const {
const_iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
iterator insert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(tryInsert(key_order));
assert(it_ok.second);
return it_ok.first; }
iterator insert(const Symbol& key, varid_t order) { return insert(std::make_pair(key,order)); }
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
std::pair<iterator,bool> tryInsert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(order_.insert(key_order));
if(key_order.second+1 > nVars_) nVars_ = key_order.second+1;
return it_ok; }
std::pair<iterator,bool> tryInsert(const Symbol& key, varid_t order) { return tryInsert(std::make_pair(key,order)); }
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
varid_t push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
/**
* += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
@ -95,19 +95,19 @@ public:
* @class Unordered
* @brief a set of unordered indice
*/
class Unordered: public std::set<varid_t>, public Testable<Unordered> {
class Unordered: public std::set<Index>, public Testable<Unordered> {
public:
/** Default constructor creates empty ordering */
Unordered() { }
/** Create from a single symbol */
Unordered(varid_t key) { insert(key); }
Unordered(Index key) { insert(key); }
/** Copy constructor */
Unordered(const std::set<varid_t>& keys_in) : std::set<varid_t>(keys_in) {}
Unordered(const std::set<Index>& keys_in) : std::set<Index>(keys_in) {}
/** whether a key exists */
bool exists(const varid_t& key) { return find(key) != end(); }
bool exists(const Index& key) { return find(key) != end(); }
// Testable
void print(const std::string& s = "Unordered") const;

2
nonlinear/TupleValues.h
View File

@ -184,7 +184,7 @@ namespace gtsam {
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(varid_t firstVar = 0) const {
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
// Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);

2
slam/GaussianISAM2.cpp
View File

@ -39,7 +39,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
bool found = true;
if (!redo && cg->nrParents()>0) {
found = false;
BOOST_FOREACH(const varid_t& key, cg->parents()) {
BOOST_FOREACH(const Index& key, cg->parents()) {
if (changed[key]) {
found = true;
}

8
slam/LinearApproxFactor-inl.h
View File

@ -22,7 +22,7 @@ LinearApproxFactor<Values,Key>::LinearApproxFactor(
{
BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) {
Symbol key = p.first;
varid_t var = p.second;
Index var = p.second;
// check if actually in factor
Factor::const_iterator it = lin_factor->find(var);
@ -60,13 +60,13 @@ boost::shared_ptr<GaussianFactor>
LinearApproxFactor<Values,Key>::linearize(const Values& c, const Ordering& ordering) const {
// sort by varid - only known at linearization time
typedef std::map<varid_t, Matrix> VarMatrixMap;
typedef std::map<Index, Matrix> VarMatrixMap;
VarMatrixMap sorting_terms;
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_)
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
// move into terms
std::vector<std::pair<varid_t, Matrix> > terms;
std::vector<std::pair<Index, Matrix> > terms;
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
terms.push_back(p);
@ -77,7 +77,7 @@ LinearApproxFactor<Values,Key>::linearize(const Values& c, const Ordering& order
template <class Values, class Key>
Factor::shared_ptr
LinearApproxFactor<Values,Key>::symbolic(const Ordering& ordering) const {
std::vector<varid_t> key_ids(this->keys_.size());
std::vector<Index> key_ids(this->keys_.size());
size_t i=0;
BOOST_FOREACH(const Symbol& key, this->keys_)
key_ids[i++] = ordering[key];

4
slam/smallExample.cpp
View File

@ -35,8 +35,8 @@ namespace example {
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const varid_t _l1_=0, _x1_=1, _x2_=2;
static const varid_t _x_=0, _y_=1, _z_=2;
static const Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2;
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {

2
tests/testGaussianBayesNet.cpp
View File

@ -30,7 +30,7 @@ using namespace std;
using namespace gtsam;
using namespace example;
static const varid_t _x_=0, _y_=1, _z_=2;
static const Index _x_=0, _y_=1, _z_=2;
/* ************************************************************************* */
TEST( GaussianBayesNet, constructor )

16
tests/testGaussianJunctionTree.cpp
View File

@ -43,14 +43,14 @@ TEST( GaussianJunctionTree, constructor2 )
// create an ordering
GaussianJunctionTree actual(fg);
vector<varid_t> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
vector<varid_t> frontal2; frontal2 += ordering["x3"], ordering["x2"];
vector<varid_t> frontal3; frontal3 += ordering["x1"];
vector<varid_t> frontal4; frontal4 += ordering["x7"];
vector<varid_t> sep1;
vector<varid_t> sep2; sep2 += ordering["x4"];
vector<varid_t> sep3; sep3 += ordering["x2"];
vector<varid_t> sep4; sep4 += ordering["x6"];
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"];
CHECK(assert_equal(frontal1, actual.root()->frontal));
CHECK(assert_equal(sep1, actual.root()->separator));
LONGS_EQUAL(5, actual.root()->size());

6
tests/testInference.cpp
View File

@ -29,8 +29,8 @@ TEST(GaussianFactorGraph, createSmoother)
LONGS_EQUAL(5,fg2.size());
// eliminate
list<varid_t> x3var; x3var.push_back(ordering["x3"]);
list<varid_t> x1var; x1var.push_back(ordering["x1"]);
list<Index> x3var; x3var.push_back(ordering["x3"]);
list<Index> x1var; x1var.push_back(ordering["x1"]);
GaussianBayesNet p_x3 = *Inference::Marginal(fg2, x3var);
GaussianBayesNet p_x1 = *Inference::Marginal(fg2, x1var);
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
@ -41,7 +41,7 @@ TEST( Inference, marginals )
{
// create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet();
list<varid_t> xvar; xvar.push_back(0);
list<Index> xvar; xvar.push_back(0);
GaussianBayesNet actual = *Inference::Marginal(GaussianFactorGraph(cbn), xvar);
// expected is just scalar Gaussian on x