diff --git a/base/TestableAssertions.h b/base/TestableAssertions.h index 35363d6176..9fb4bbf020 100644 --- a/base/TestableAssertions.h +++ b/base/TestableAssertions.h @@ -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; diff --git a/base/types.h b/base/types.h index c3661cb837..9299804f6d 100644 --- a/base/types.h +++ b/base/types.h @@ -11,37 +11,48 @@ namespace gtsam { -typedef size_t varid_t; - -/** Helper class that uses templates to select between two types based on - * whether TEST_TYPE is const or not. - */ -template -struct const_selector {}; - -/** Specialization for the non-const version */ -template -struct const_selector { - typedef AS_NON_CONST type; -}; -/** Specialization for the const version */ -template -struct const_selector { - 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 -struct ValueWithDefault { - T value; - ValueWithDefault() : value(defaultValue) {} - ValueWithDefault(const T& _value) : value(_value) {} - T& operator*() { return value; } -}; + /** + * 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 + struct const_selector { + }; + + /** Specialization for the non-const version */ + template + struct const_selector { + typedef AS_NON_CONST type; + }; + /** Specialization for the const version */ + template + struct const_selector { + 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 + struct ValueWithDefault { + T value; + ValueWithDefault() : + value(defaultValue) { + } + ValueWithDefault(const T& _value) : + value(_value) { + } + T& operator*() { + return value; + } + }; } diff --git a/geometry/tensors.h b/geometry/tensors.h index 36f00b6ab1..52282ca701 100644 --- a/geometry/tensors.h +++ b/geometry/tensors.h @@ -11,7 +11,7 @@ namespace tensors { /** index */ template struct Index { - enum { dim = Dim }; + static const int dim = Dim; }; } // namespace tensors diff --git a/inference/BayesNet-inl.h b/inference/BayesNet-inl.h index f7060c2845..a9d484feea 100644 --- a/inference/BayesNet-inl.h +++ b/inference/BayesNet-inl.h @@ -75,8 +75,8 @@ namespace gtsam { /* ************************************************************************* */ template - list BayesNet::ordering() const { - list ord; + list BayesNet::ordering() const { + list 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 typename BayesNet::sharedConditional - BayesNet::operator[](varid_t key) const { + BayesNet::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())); diff --git a/inference/BayesNet.h b/inference/BayesNet.h index 4747b455d2..c8650e0fb8 100644 --- a/inference/BayesNet.h +++ b/inference/BayesNet.h @@ -97,10 +97,10 @@ namespace gtsam { } /** return keys in reverse topological sort order, i.e., elimination order */ - std::list ordering() const; + std::list 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(); } diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 7198662cf2..1608e2f705 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -76,8 +76,8 @@ namespace gtsam { /* ************************************************************************* */ template - vector BayesTree::Clique::keys() const { - vector keys; + vector BayesTree::Clique::keys() const { + vector 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 void BayesTree::Clique::permuteWithInverse(const Permutation& inversePermutation) { BayesNet::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::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, boost::fast_pool_allocator > FastJSet; + typedef set, boost::fast_pool_allocator > FastJSet; // Get the key list Cp=Fp+Sp, which will form the basis for the integrands - vector parentKeys(parent->keys()); + vector 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 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 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 p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); // remove all integrands - for(varid_t j=0; jkeys()) 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::sharedClique BayesTree::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::sharedClique BayesTree::addClique( const sharedConditional& conditional, list& 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::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 template - inline varid_t BayesTree::findParentClique(const Container& parents) const { + inline Index BayesTree::findParentClique(const Container& parents) const { typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; -// boost::optional parentCliqueRepresentative; +// boost::optional parentCliqueRepresentative; // boost::optional 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::const_iterator parent = parents.begin(); +// list::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 template FactorGraph - BayesTree::marginal(varid_t key) const { + BayesTree::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 keyAsVector(1); keyAsVector[0] = key; + vector 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 template BayesNet - BayesTree::marginalBayesNet(varid_t key) const { + BayesTree::marginalBayesNet(Index key) const { // calculate marginal as a factor graph FactorGraph fg = this->marginal(key); @@ -728,7 +728,7 @@ namespace gtsam { // template // template // FactorGraph -// BayesTree::joint(varid_t key1, varid_t key2) const { +// BayesTree::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 // template // BayesNet -// BayesTree::jointBayesNet(varid_t key1, varid_t key2) const { +// BayesTree::jointBayesNet(Index key1, Index key2) const { // // // calculate marginal as a factor graph // FactorGraph fg = this->joint(key1,key2); @@ -803,7 +803,7 @@ namespace gtsam { BayesNet& bn, typename BayesTree::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()) { diff --git a/inference/BayesTree.h b/inference/BayesTree.h index 6523dd14d7..28263509ac 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -45,7 +45,7 @@ namespace gtsam { typedef typename boost::weak_ptr weak_ptr; weak_ptr parent_; std::list children_; - std::list separator_; /** separator keys */ + std::list separator_; /** separator keys */ typename Conditional::FactorType::shared_ptr cachedFactor_; friend class BayesTree; @@ -58,7 +58,7 @@ namespace gtsam { Clique(const BayesNet& bayesNet); /** return keys in frontal:separator order */ - std::vector keys() const; + std::vector 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 - 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 - FactorGraph marginal(varid_t key) const; + FactorGraph marginal(Index key) const; /** return marginal on any variable, as a Bayes Net */ template - BayesNet marginalBayesNet(varid_t key) const; + BayesNet marginalBayesNet(Index key) const; // /** return joint on two variables */ // template -// FactorGraph joint(varid_t key1, varid_t key2) const; +// FactorGraph joint(Index key1, Index key2) const; // // /** return joint on two variables as a BayesNet */ // template -// BayesNet jointBayesNet(varid_t key1, varid_t key2) const; +// BayesNet jointBayesNet(Index key1, Index key2) const; /** * Read only with side effects diff --git a/inference/ClusterTree-inl.h b/inference/ClusterTree-inl.h index c970ba7dc5..55125b24a8 100644 --- a/inference/ClusterTree-inl.h +++ b/inference/ClusterTree-inl.h @@ -21,7 +21,7 @@ namespace gtsam { * ************************************************************************* */ template template - ClusterTree::Cluster::Cluster(const FG& fg, varid_t key, Iterator firstSeparator, Iterator lastSeparator) : + ClusterTree::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 void ClusterTree::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; } diff --git a/inference/ClusterTree.h b/inference/ClusterTree.h index 9789a6c649..7f1c33653f 100644 --- a/inference/ClusterTree.h +++ b/inference/ClusterTree.h @@ -36,8 +36,8 @@ namespace gtsam { typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; - const std::vector frontal; // the frontal variables - const std::vector separator; // the separator variables + const std::vector frontal; // the frontal variables + const std::vector separator; // the separator variables protected: @@ -52,7 +52,7 @@ namespace gtsam { /* Create a node with a single frontal variable */ template - 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 diff --git a/inference/Conditional.h b/inference/Conditional.h index 8d763cd2f4..cdb71f329d 100644 --- a/inference/Conditional.h +++ b/inference/Conditional.h @@ -56,19 +56,19 @@ class Conditional: boost::noncopyable, public Testable { 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& parents) : nFrontal_(1) { + Conditional(Index key, const std::vector& parents) : nFrontal_(1) { factor_.keys_.resize(1+parents.size()); *(beginFrontals()) = key; std::copy(parents.begin(), parents.end(), beginParents()); } @@ -78,7 +78,7 @@ class Conditional: boost::noncopyable, public Testable { 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 @@ class Conditional: boost::noncopyable, public Testable { */ 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 @@ class Conditional: boost::noncopyable, public Testable { } /** return a const reference to all keys */ - const std::vector& keys() const { return factor_.keys(); } + const std::vector& keys() const { return factor_.keys(); } /** return a view of the frontal keys */ Frontals frontals() const { @@ -126,9 +126,9 @@ class Conditional: boost::noncopyable, public Testable { /** 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; } diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index 46e8bd3906..0637e00fcf 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -19,7 +19,7 @@ namespace gtsam { /* ************************************************************************* */ // template -// void EliminationTree::add(const FG& fg, varid_t j) { +// void EliminationTree::add(const FG& fg, Index j) { // sharedNode node(new Node(fg, j)); // add(node); // } @@ -29,7 +29,7 @@ namespace gtsam { void EliminationTree::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::const_iterator parentIndex = min_element(node->separator.begin(), node->separator.end()); + vector::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 > VariableSlots; - map > variableSlots; + typedef map > VariableSlots; + map > 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 empty; + static vector empty; VariableSlots::iterator thisVarSlots = variableSlots.insert(make_pair(involvedVariable,empty)).first; if(thisVarSlots->second.empty()) - thisVarSlots->second.resize(involvedFactors.size(), numeric_limits::max()); + thisVarSlots->second.resize(involvedFactors.size(), numeric_limits::max()); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor " << factorI << " slot " << factorVarSlot << endl; diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index 52a6edd815..87fa164f56 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -31,7 +31,7 @@ template class _EliminationTreeTester; typedef typename Node::shared_ptr sharedNode; // we typedef the following handy list of ordered factor graphs - typedef std::pair NamedGraph; + typedef std::pair NamedGraph; typedef std::list OrderedGraphs; private: @@ -47,7 +47,7 @@ template 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. diff --git a/inference/Factor-inl.h b/inference/Factor-inl.h index 00e23692fb..6903e23af2 100644 --- a/inference/Factor-inl.h +++ b/inference/Factor-inl.h @@ -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 Factor::Factor(KeyIterator beginKey, KeyIterator end template Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph, const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions) { + const std::vector& variables, const std::vector >& variablePositions) { return shared_ptr(boost::make_shared(variables.begin(), variables.end())); } /* ************************************************************************* */ template -Factor::shared_ptr Factor::Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots) { - typedef const std::map, std::less, MapAllocator> VariableSlots; +Factor::shared_ptr Factor::Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots) { + typedef const std::map, std::less, 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); diff --git a/inference/Factor.cpp b/inference/Factor.cpp index a646f81302..88b3fd02bb 100644 --- a/inference/Factor.cpp +++ b/inference/Factor.cpp @@ -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 > Factor::eliminate(varid_t nFrontals) { +boost::shared_ptr > Factor::eliminate(Index nFrontals) { assert(keys_.size() >= nFrontals); checkSorted(); BayesNet::shared_ptr fragment(new BayesNet()); 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 > 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]; } } } diff --git a/inference/Factor.h b/inference/Factor.h index 8979ea0485..9c407709b0 100644 --- a/inference/Factor.h +++ b/inference/Factor.h @@ -39,7 +39,7 @@ class Conditional; class Factor : public Testable { protected: - std::vector keys_; + std::vector keys_; ValueWithDefault permuted_; /** Internal check to make sure keys are sorted (invariant during elimination). @@ -50,8 +50,8 @@ class Factor : public Testable { typedef gtsam::Conditional Conditional; typedef boost::shared_ptr shared_ptr; - typedef std::vector::iterator iterator; - typedef std::vector::const_iterator const_iterator; + typedef std::vector::iterator iterator; + typedef std::vector::const_iterator const_iterator; /** Copy constructor */ Factor(const Factor& f); @@ -66,19 +66,19 @@ class Factor : public Testable { 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 @@ class Factor : public Testable { template static shared_ptr Combine(const FactorGraphType& factorGraph, const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions); + const std::vector& variables, const std::vector >& variablePositions); /** Create a combined joint factor (new style for EliminationTree). */ template - static shared_ptr Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots); + static shared_ptr Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots); /** * eliminate the first variable involved in this factor @@ -102,7 +102,7 @@ class Factor : public Testable { /** * eliminate the first nFrontals frontal variables. */ - boost::shared_ptr > eliminate(varid_t nFrontals = 1); + boost::shared_ptr > eliminate(Index nFrontals = 1); /** * Permutes the GaussianFactor, but for efficiency requires the permutation @@ -119,13 +119,13 @@ class Factor : public Testable { 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 @@ class Factor : public Testable { /** * return keys in order as created */ - const std::vector& keys() const { return keys_; } + const std::vector& keys() const { return keys_; } /** * @return the number of nodes the factor connects diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index fb79554bf2..9acdeb520e 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -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 >& columns, -// Ordering& ordering, const set& lastKeys) { +// static void colamd(int n_col, int n_row, int nrNonZeros, const map >& columns, +// Ordering& ordering, const set& 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 >::const_iterator iterator; +// typedef map >::const_iterator iterator; // bool front_exists = false; -// vector initialOrder; +// vector initialOrder; // for (iterator it = columns.begin(); it != columns.end(); it++) { -// varid_t key = it->first; +// Index key = it->first; // const vector& column = it->second; // initialOrder.push_back(key); // BOOST_FOREACH(int i, column) @@ -159,12 +159,12 @@ namespace gtsam { // /* ************************************************************************* */ // template // void FactorGraph::getOrdering(Ordering& ordering, -// const set& lastKeys, -// boost::optional&> scope) const { +// const set& lastKeys, +// boost::optional&> 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 > columns; // map from keys to a sparse column of non-zero row indices +// map > 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& keys(factor->keys()); +// const vector& 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 // Ordering FactorGraph::getOrdering() const { // Ordering ordering; -// set lastKeys; +// set lastKeys; // getOrdering(ordering, lastKeys); // return ordering; // } @@ -201,16 +201,16 @@ namespace gtsam { // template // boost::shared_ptr FactorGraph::getOrdering_() const { // boost::shared_ptr ordering(new Ordering); -// set lastKeys; +// set lastKeys; // getOrdering(*ordering, lastKeys); // return ordering; // } // // /* ************************************************************************* */ // template -// Ordering FactorGraph::getOrdering(const set& scope) const { +// Ordering FactorGraph::getOrdering(const set& scope) const { // Ordering ordering; -// set lastKeys; +// set lastKeys; // getOrdering(ordering, lastKeys, scope); // return ordering; // } @@ -218,7 +218,7 @@ namespace gtsam { // /* ************************************************************************* */ // template // Ordering FactorGraph::getConstrainedOrdering( -// const set& lastKeys) const { +// const set& lastKeys) const { // Ordering ordering; // getOrdering(ordering, lastKeys); // return ordering; @@ -323,14 +323,14 @@ namespace gtsam { // /* ************************************************************************* */ // template -// std::pair , set > FactorGraph::removeSingletons() { +// std::pair , set > FactorGraph::removeSingletons() { // FactorGraph singletonGraph; -// set singletons; +// set singletons; // // while (true) { // // find all the singleton variables // Ordering new_singletons; -// varid_t key; +// Index key; // list 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 boost::shared_ptr removeAndCombineFactors( -// FactorGraph& factorGraph, const varid_t& key) { +// FactorGraph& factorGraph, const Index& key) { // // // find and remove the factors associated with key // vector > found = factorGraph.findAndRemoveFactors(key); diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 1919a7b6dc..4f715184af 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -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& lastKeys, -// boost::optional&> scope = boost::none) const; +// void getOrdering(Ordering& ordering, const std::set& lastKeys, +// boost::optional&> scope = boost::none) const; public: @@ -117,8 +117,8 @@ namespace gtsam { // */ // Ordering getOrdering() const; // boost::shared_ptr getOrdering_() const; -// Ordering getOrdering(const std::set& scope) const; -// Ordering getConstrainedOrdering(const std::set& lastKeys) const; +// Ordering getOrdering(const std::set& scope) const; +// Ordering getConstrainedOrdering(const std::set& 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 findAndRemoveFactors(varid_t key); +// std::vector findAndRemoveFactors(Index key); // // /** remove singleton variables and the related factors */ -// std::pair, std::set > removeSingletons(); +// std::pair, std::set > removeSingletons(); private: @@ -198,7 +198,7 @@ namespace gtsam { // * @return the combined linear factor // */ // template boost::shared_ptr -// removeAndCombineFactors(FactorGraph& factorGraph, const varid_t& key); +// removeAndCombineFactors(FactorGraph& factorGraph, const Index& key); /** diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index e474bf1438..5768db3614 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -49,15 +49,15 @@ ISAM2::ISAM2() : BayesTree(), delta_(Permutati /* ************************************************************************* */ template -list ISAM2::getAffectedFactors(const list& keys) const { +list ISAM2::getAffectedFactors(const list& 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 > allAffected; list indices; - BOOST_FOREACH(const varid_t key, keys) { + BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); // indices.insert(indices.begin(), l.begin(), l.end()); const VariableIndexType::mapped_type& factors(variableIndex_[key]); @@ -79,7 +79,7 @@ list ISAM2::getAffectedFactors(const list& // (note that the remaining stuff is summarized in the cached factors) template boost::shared_ptr ISAM2::relinearizeAffectedFactors -(const list& affectedKeys) const { +(const list& affectedKeys) const { tic("8.2.2.1 getAffectedFactors"); list candidates = getAffectedFactors(affectedKeys); @@ -89,7 +89,7 @@ boost::shared_ptr ISAM2::relinearizeAf tic("8.2.2.2 affectedKeysSet"); // for fast lookup below - set affectedKeysSet; + set affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); toc("8.2.2.2 affectedKeysSet"); @@ -97,7 +97,7 @@ boost::shared_ptr ISAM2::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::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::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::sharedClique& root, } template -boost::shared_ptr > ISAM2::recalculate(const set& markedKeys, const vector& newKeys, const GaussianFactorGraph* newFactors) { +boost::shared_ptr > ISAM2::recalculate(const set& markedKeys, const vector& newKeys, const GaussianFactorGraph* newFactors) { static const bool debug = false; static const bool useMultiFrontal = true; @@ -198,7 +198,7 @@ boost::shared_ptr > ISAM2::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 > ISAM2::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 affectedKeys = affectedBayesNet.ordering(); + list affectedKeys = affectedBayesNet.ordering(); toc("8.2.1 re-lookup: affectedKeys"); //#ifndef NDEBUG -// varid_t lastKey; -// for(list::const_iterator key=affectedKeys.begin(); key!=affectedKeys.end(); ++key) { +// Index lastKey; +// for(list::const_iterator key=affectedKeys.begin(); key!=affectedKeys.end(); ++key) { // if(key != affectedKeys.begin()) // assert(*key > lastKey); // lastKey = *key; // } //#endif - list affectedAndNewKeys; + list 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 > ISAM2::recalculate(const s // The relinearized variables should not appear anywhere in the orphans BOOST_FOREACH(boost::shared_ptr::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 > ISAM2::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 > ISAM2::recalculate(const s toc("8.2 re-lookup"); //#ifndef NDEBUG -// for(varid_t var=0; varbegin(), cached_[var]->end(), var) == cached_[var]->end()); @@ -292,7 +292,7 @@ boost::shared_ptr > ISAM2::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 > ISAM2::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 > affectedKeysSet(new set(markedKeys)); + boost::shared_ptr > affectedKeysSet(new set(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::const_iterator key=affectedKeysSet->begin(); key!=affectedKeysSet->end(); ++key) { +// for(set::const_iterator key=affectedKeysSet->begin(); key!=affectedKeysSet->end(); ++key) { // if(key != affectedKeysSet->begin()) { -// set::const_iterator prev = key; --prev; +// set::const_iterator prev = key; --prev; // assert(*prev == *key - 1); // } // } @@ -348,7 +348,7 @@ boost::shared_ptr > ISAM2::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 > ISAM2::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::max()); + std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits::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 > ISAM2::recalculate(const s #ifdef PRESORT_ALPHA Permutation alphaOrder(affectedKeysSet->size()); vector 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 markedKeysSelected; markedKeysSelected.reserve(markedKeys.size()); - BOOST_FOREACH(varid_t var, markedKeys) { markedKeysSelected.push_back(alphaOrder[affectedKeysSelectorInverse[var]]); } + vector 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 markedKeysSelected; markedKeysSelected.reserve(markedKeys.size()); -// BOOST_FOREACH(varid_t var, markedKeys) { markedKeysSelected.push_back(affectedKeysSelectorInverse[var]); } - vector newKeysSelected; newKeysSelected.reserve(newKeys.size()); - BOOST_FOREACH(varid_t var, newKeys) { newKeysSelected.push_back(affectedKeysSelectorInverse[var]); } +// vector markedKeysSelected; markedKeysSelected.reserve(markedKeys.size()); +// BOOST_FOREACH(Index var, markedKeys) { markedKeysSelected.push_back(affectedKeysSelectorInverse[var]); } + vector 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 > ISAM2::recalculate(const s tic("8.6 eliminate"); boost::shared_ptr bayesNet(new GaussianBayesNet()); vector newlyCached(affectedKeysSelector.size()); - for(varid_t var=0; var > ISAM2::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 > ISAM2::recalculate(const s // Output: BayesTree(this) -// boost::shared_ptr > affectedKeysSet(new set()); +// boost::shared_ptr > affectedKeysSet(new set()); // affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); return affectedKeysSet; } @@ -565,17 +565,17 @@ boost::shared_ptr > ISAM2::recalculate(const s ///* ************************************************************************* */ //template //void ISAM2::linear_update(const GaussianFactorGraph& newFactors) { -// const list markedKeys = newFactors.keys(); +// const list markedKeys = newFactors.keys(); // recalculate(markedKeys, &newFactors); //} /* ************************************************************************* */ // find all variables that are directly connected by a measurement to one of the marked variables template -void ISAM2::find_all(sharedClique clique, set& keys, const vector& markedMask) { +void ISAM2::find_all(sharedClique clique, set& keys, const vector& 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 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 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 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::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 dims(newTheta.dims(*newTheta.orderingArbitrary(ordering_.nVars()))); + vector 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::update( tic("3 step3"); // 3. Mark linear update - set markedKeys; - vector newKeys; newKeys.reserve(newFactors.size() * 6); + set markedKeys; + vector newKeys; newKeys.reserve(newFactors.size() * 6); BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, newFactors) { BOOST_FOREACH(const Symbol& key, factor->keys()) { markedKeys.insert(ordering_[key]); newKeys.push_back(ordering_[key]); } } -// list markedKeys = newFactors.keys(); +// list markedKeys = newFactors.keys(); toc("3 step3"); #ifdef SEPARATE_STEPS // original algorithm from paper: separate relin and optimize @@ -705,7 +705,7 @@ void ISAM2::update( boost::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); variableIndex_.augment(*linearFactors); - boost::shared_ptr > replacedKeys_todo = recalculate(markedKeys, newKeys, &(*linearFactors)); + boost::shared_ptr > replacedKeys_todo = recalculate(markedKeys, newKeys, &(*linearFactors)); markedKeys.clear(); vector none(variableIndex_.size(), false); optimize2(this->root(), wildfire_threshold, none, delta_); @@ -716,7 +716,7 @@ void ISAM2::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= relinearize_threshold) { markedRelinMask[var] = true; markedKeys.insert(var); @@ -742,7 +742,7 @@ void ISAM2::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::update( tic("8 step8"); // 8. Redo top of Bayes tree - boost::shared_ptr > replacedKeys = recalculate(markedKeys, newKeys, &(*linearFactors)); + boost::shared_ptr > replacedKeys = recalculate(markedKeys, newKeys, &(*linearFactors)); toc("8 step8"); #else - vector empty; - boost::shared_ptr > replacedKeys = recalculate(markedKeys, empty); + vector empty; + boost::shared_ptr > replacedKeys = recalculate(markedKeys, empty); #endif tic("9 step9"); @@ -812,7 +812,7 @@ void ISAM2::update( } else { vector 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"); diff --git a/inference/ISAM2.h b/inference/ISAM2.h index ffd7e5b926..4ff2bae0ff 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -52,7 +52,7 @@ class ISAM2: public BayesTree { // for keeping all original nonlinear factors NonlinearFactorGraph 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 @@ class ISAM2: public BayesTree { private: - std::list getAffectedFactors(const std::list& keys) const; - boost::shared_ptr relinearizeAffectedFactors(const std::list& affectedKeys) const; + std::list getAffectedFactors(const std::list& keys) const; + boost::shared_ptr relinearizeAffectedFactors(const std::list& affectedKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - boost::shared_ptr > recalculate(const std::set& markedKeys, const std::vector& newKeys, const GaussianFactorGraph* newFactors = NULL); + boost::shared_ptr > recalculate(const std::set& markedKeys, const std::vector& newKeys, const GaussianFactorGraph* newFactors = NULL); // void linear_update(const GaussianFactorGraph& newFactors); - void find_all(sharedClique clique, std::set& keys, const std::vector& marked); // helper function + void find_all(sharedClique clique, std::set& keys, const std::vector& marked); // helper function }; // ISAM2 diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index cbdd40125e..f69e68a62b 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -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 lowestOrdered(fg.size()); - varid_t maxVar = 0; + vector lowestOrdered(fg.size()); + Index maxVar = 0; for(size_t i=0; ibegin(), fg[i]->end()); if(min == fg[i]->end()) - lowestOrdered[i] = numeric_limits::max(); + lowestOrdered[i] = numeric_limits::max(); else { lowestOrdered[i] = *min; maxVar = std::max(maxVar, *min); @@ -74,7 +74,7 @@ namespace gtsam { // variable. vector > > targets(maxVar+1); for(size_t i=0; i::max()) + if(lowestOrdered[i] != numeric_limits::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 frontals = bayesClique->ordering(); + list 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 allKeys; + list allKeys; allKeys.insert(allKeys.end(), current->frontal.begin(), current->frontal.end()); allKeys.insert(allKeys.end(), current->separator.begin(), current->separator.end()); - vector varslotsKeys(variableSlots.size()); + vector 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())); diff --git a/inference/Permutation.cpp b/inference/Permutation.cpp index af3b9eb302..9ce5378689 100644 --- a/inference/Permutation.cpp +++ b/inference/Permutation.cpp @@ -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& toFront, size_t size) { +Permutation Permutation::PullToFront(const vector& toFront, size_t size) { Permutation ret(size); @@ -32,14 +32,14 @@ Permutation Permutation::PullToFront(const vector& 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_t size } /* ************************************************************************* */ -Permutation Permutation::PushToBack(const std::vector& toBack, size_t size) { +Permutation Permutation::PushToBack(const std::vector& toBack, size_t size) { Permutation ret(size); @@ -57,8 +57,8 @@ Permutation Permutation::PushToBack(const std::vector& 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_t s // Fill in the rest of the variables nextVar = 0; - for(varid_t j=0; jsize())); - for(varid_t i=0; isize(); ++i) { + for(Index i=0; isize(); ++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; } diff --git a/inference/Permutation.h b/inference/Permutation.h index 28a5a6ab4e..8493aff25b 100644 --- a/inference/Permutation.h +++ b/inference/Permutation.h @@ -34,12 +34,12 @@ class Inference; */ class Permutation { protected: - std::vector rangeIndices_; + std::vector rangeIndices_; public: typedef boost::shared_ptr shared_ptr; - typedef std::vector::const_iterator const_iterator; - typedef std::vector::iterator iterator; + typedef std::vector::const_iterator const_iterator; + typedef std::vector::iterator iterator; /** * Create an empty permutation. This cannot do anything, but you can later @@ -51,20 +51,20 @@ class Permutation { * 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 @@ class Permutation { /** * 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& toFront, size_t size); + static Permutation PullToFront(const std::vector& 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& toBack, size_t size); + static Permutation PushToBack(const std::vector& toBack, size_t size); iterator begin() { return rangeIndices_.begin(); } const_iterator begin() const { return rangeIndices_.begin(); } @@ -136,7 +136,7 @@ class Permutation { 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; }; diff --git a/inference/SymbolicFactorGraph.cpp b/inference/SymbolicFactorGraph.cpp index a600ad3f08..2e08ed98a3 100644 --- a/inference/SymbolicFactorGraph.cpp +++ b/inference/SymbolicFactorGraph.cpp @@ -27,33 +27,33 @@ namespace gtsam { FactorGraph(bayesNet) {} /* ************************************************************************* */ - void SymbolicFactorGraph::push_factor(varid_t key) { + void SymbolicFactorGraph::push_factor(Index key) { boost::shared_ptr 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(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(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(new Factor(key1,key2,key3,key4)); push_back(factor); } /* ************************************************************************* */ - std::set, boost::fast_pool_allocator > + std::set, boost::fast_pool_allocator > SymbolicFactorGraph::keys() const { - std::set, boost::fast_pool_allocator > keys; + std::set, boost::fast_pool_allocator > keys; BOOST_FOREACH(const sharedFactor& factor, *this) { if(factor) keys.insert(factor->begin(), factor->end()); } return keys; diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 56fa90a3cb..ef5d2cb46e 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -32,16 +32,16 @@ class SymbolicFactorGraph: public FactorGraph { SymbolicFactorGraph(const BayesNet& 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 @@ class SymbolicFactorGraph: public FactorGraph { * Return the set of variables involved in the factors (computes a set * union). */ - std::set, boost::fast_pool_allocator > keys() const; + std::set, boost::fast_pool_allocator > keys() const; /** * Same as eliminate in the SymbolicFactorGraph case diff --git a/inference/VariableIndex.h b/inference/VariableIndex.h index 8537a4ae22..c83fb797b6 100644 --- a/inference/VariableIndex.h +++ b/inference/VariableIndex.h @@ -63,11 +63,11 @@ class VariableIndex { VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} template 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 void augment(const Derived& varIndex); template void augment(const FactorGraph& factorGraph); @@ -78,12 +78,12 @@ class VariableIndex { 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::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; jindex_.size(); ++j) + for(Index j=0; jindex_.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; jindex_[j].swap(original[permutation[j]]); } @@ -115,10 +115,10 @@ VariableIndex::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::VariableIndex(const FactorGraph& factorGraph) : // Build index mapping from variable id to factor index for(size_t fi=0; fikeys()) { + 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::VariableIndex(const FactorGraph& factorGraph) : //void VariableIndex::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::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::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; varkeys()) { + 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::equals(const Derived& other, double tol) const { template void VariableIndex::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()); diff --git a/inference/VariableSlots-inl.h b/inference/VariableSlots-inl.h index 90428d9209..d1b8456a38 100644 --- a/inference/VariableSlots-inl.h +++ b/inference/VariableSlots-inl.h @@ -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())); + boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, vector())); if(inserted) - thisVarSlots->second.resize(factorGraph.size(), numeric_limits::max()); + thisVarSlots->second.resize(factorGraph.size(), numeric_limits::max()); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << endl; ++ factorVarSlot; diff --git a/inference/VariableSlots.cpp b/inference/VariableSlots.cpp index 475873b803..3cbab206dd 100644 --- a/inference/VariableSlots.cpp +++ b/inference/VariableSlots.cpp @@ -27,7 +27,7 @@ void VariableSlots::print(const std::string& str) const { for(size_t i=0; ibegin()->second.size(); ++i) { cout << " \t"; BOOST_FOREACH(const value_type& slot, *this) { - if(slot.second[i] == numeric_limits::max()) + if(slot.second[i] == numeric_limits::max()) cout << "x" << "\t"; else cout << slot.second[i] << "\t"; diff --git a/inference/VariableSlots.h b/inference/VariableSlots.h index a8c8ea6c09..d675b9bde0 100644 --- a/inference/VariableSlots.h +++ b/inference/VariableSlots.h @@ -24,7 +24,7 @@ namespace gtsam { * interleaved. * * VariableSlots describes the 2D block structure of the combined factor. It - * is essentially a map >. The varid_t is the real + * is essentially a map >. The Index is the real * variable index of the combined factor slot. The vector 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, std::less, - boost::fast_pool_allocator > > > _VariableSlots_map; +typedef std::map, std::less, + boost::fast_pool_allocator > > > _VariableSlots_map; class VariableSlots : public _VariableSlots_map, public Testable { diff --git a/inference/inference-inl.h b/inference/inference-inl.h index 55cd5847a0..22f543e552 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -48,7 +48,7 @@ BayesNet::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 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 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 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, boost::fast_pool_allocator > > involvedKeys; // Variable and original order as discovered + map, boost::fast_pool_allocator > > 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 varposPermutation(involvedKeys.size(), numeric_limits::max()); - vector sortedKeys(involvedKeys.size()); + vector sortedKeys(involvedKeys.size()); { size_t sortedVarpos = 0; - const map, boost::fast_pool_allocator > >& involvedKeysC(involvedKeys); - for(map, boost::fast_pool_allocator > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) { + const map, boost::fast_pool_allocator > >& involvedKeysC(involvedKeys); + for(map, boost::fast_pool_allocator > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) { sortedKeys[sortedVarpos] = key_pos->first; assert(varposPermutation[key_pos->second] == numeric_limits::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; sortedPoskeys()) { + 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; jpush_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::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 -// boost::shared_ptr eliminateOne(FactorGraph& graph, varid_t key) { +// boost::shared_ptr eliminateOne(FactorGraph& 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 bayesNet; // empty // -// BOOST_FOREACH(varid_t key, ordering) { +// BOOST_FOREACH(Index key, ordering) { // boost::shared_ptr cg = eliminateOne(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 = eliminate(factorGraph,ord); diff --git a/inference/inference.cpp b/inference/inference.cpp index 2b3cb7ec4a..bf05409c2a 100644 --- a/inference/inference.cpp +++ b/inference/inference.cpp @@ -13,7 +13,7 @@ namespace gtsam { /* ************************************************************************* */ Conditional::shared_ptr -Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, varid_t var) { +Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var) { tic("EliminateOne"); @@ -42,7 +42,7 @@ Inference::EliminateOneSymbolic(FactorGraph& 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, boost::fast_pool_allocator > InvolvedKeys; + typedef set, boost::fast_pool_allocator > InvolvedKeys; InvolvedKeys involvedKeys; BOOST_FOREACH(size_t removedFactorI, removedFactors) { if(debug) cout << removedFactorI << " is involved" << endl; @@ -50,7 +50,7 @@ Inference::EliminateOneSymbolic(FactorGraph& 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()); diff --git a/inference/inference.h b/inference/inference.h index db3a2fc830..6a08fd1896 100644 --- a/inference/inference.h +++ b/inference/inference.h @@ -58,7 +58,7 @@ class Conditional; */ template 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 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 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 - EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, varid_t var); + EliminateOneSymbolic(FactorGraph& 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 - static boost::shared_ptr PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector()); } + static boost::shared_ptr PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector()); } template static boost::shared_ptr PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast); @@ -125,7 +125,7 @@ class Conditional; // */ // template // boost::shared_ptr -// eliminateOne(FactorGraph& factorGraph, varid_t key); +// eliminateOne(FactorGraph& factorGraph, Index key); // // /** // * eliminate factor graph using the given (not necessarily complete) diff --git a/inference/tests/testBayesTree.cpp b/inference/tests/testBayesTree.cpp index 26cd477af8..21e07fe88a 100644 --- a/inference/tests/testBayesTree.cpp +++ b/inference/tests/testBayesTree.cpp @@ -23,7 +23,7 @@ typedef BayesTree 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 index(ordering); - list parents1; parents1 += _E_, _L_; + list parents1; parents1 += _E_, _L_; CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1))); - list parents2; parents2 += _L_, _E_; + list parents2; parents2 += _L_, _E_; CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2))); - list parents3; parents3 += _L_, _B_; + list 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 bn; SymbolicBayesTree::Cliques orphans; - list keys; keys += _B_,_S_; + list 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 bn; SymbolicBayesTree::Cliques orphans; - list keys; keys += _B_,_S_; + list 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 keys; + list keys; keys += _l5_, _x2_, _x3_, _x4_; BayesNet 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_); diff --git a/inference/tests/testEliminationTree.cpp b/inference/tests/testEliminationTree.cpp index fc8db53235..9a9465bf80 100644 --- a/inference/tests/testEliminationTree.cpp +++ b/inference/tests/testEliminationTree.cpp @@ -58,55 +58,76 @@ typedef EliminationTree SymbolicEliminationTree; // CHECK(assert_equal(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 c1sep; c1sep += x2,x3; +// SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end())); +// +// fc2.push_factor(x2,x5); +// list c2sep; c2sep += x3,x5; +// SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end())); +// +// fc3.push_factor(x3,x5); +// list c3sep; c3sep += x5; +// SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end())); +// +// fc4.push_factor(x4,x5); +// list c4sep; c4sep += x5; +// SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end())); +// +// list c5sep; +// SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end())); +// +// /** build expected tree using test accessor */ +// SymbolicEliminationTree expected; +// _EliminationTreeTester 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(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; - - fc1.push_factor(x1,x2); fc1.push_factor(x1,x3); - list c1sep; c1sep += x2,x3; - SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end())); - - fc2.push_factor(x2,x5); - list c2sep; c2sep += x3,x5; - SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end())); - - fc3.push_factor(x3,x5); - list c3sep; c3sep += x5; - SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end())); - - fc4.push_factor(x4,x5); - list c4sep; c4sep += x5; - SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end())); - - list c5sep; - SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end())); + // 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); - /** build expected tree using test accessor */ - SymbolicEliminationTree expected; - _EliminationTreeTester 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; + // Create elimination tree + SymbolicEliminationTree actual(fg); - /** 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 it - CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/inference/tests/testJunctionTree.cpp b/inference/tests/testJunctionTree.cpp index 044f0a4dbc..6f47ca35ad 100644 --- a/inference/tests/testJunctionTree.cpp +++ b/inference/tests/testJunctionTree.cpp @@ -33,7 +33,7 @@ typedef BayesTree 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 frontal1; frontal1 += x3, x4; - vector frontal2; frontal2 += x2, x1; - vector sep1; - vector sep2; sep2 += x3; + vector frontal1; frontal1 += x3, x4; + vector frontal2; frontal2 += x2, x1; + vector sep1; + vector 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); diff --git a/inference/tests/testSymbolicBayesNet.cpp b/inference/tests/testSymbolicBayesNet.cpp index 8d2460c4da..24d278ad9c 100644 --- a/inference/tests/testSymbolicBayesNet.cpp +++ b/inference/tests/testSymbolicBayesNet.cpp @@ -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_)), diff --git a/inference/tests/testSymbolicFactor.cpp b/inference/tests/testSymbolicFactor.cpp index 3ec1486356..1bb5884c3a 100644 --- a/inference/tests/testSymbolicFactor.cpp +++ b/inference/tests/testSymbolicFactor.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; /* ************************************************************************* */ TEST(SymbolicFactor, eliminate) { - vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; + vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; Factor actual(keys.begin(), keys.end()); BayesNet fragment = *actual.eliminate(3); diff --git a/inference/tests/testSymbolicFactorGraph.cpp b/inference/tests/testSymbolicFactorGraph.cpp index 1d9a84fc8e..3f1384a87f 100644 --- a/inference/tests/testSymbolicFactorGraph.cpp +++ b/inference/tests/testSymbolicFactorGraph.cpp @@ -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 ) diff --git a/linear/GaussianBayesNet.cpp b/linear/GaussianBayesNet.cpp index a6016b52b3..59ceef4158 100644 --- a/linear/GaussianBayesNet.cpp +++ b/linear/GaussianBayesNet.cpp @@ -26,7 +26,7 @@ template class BayesNet; 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 allocateVectorValues(const GaussianBayesNet& bn) { vector dimensions(bn.size()); - varid_t var = 0; + Index var = 0; BOOST_FOREACH(const boost::shared_ptr 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 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 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(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 mapping; + size_t N=0; map mapping; BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) { mapping.insert(make_pair(cg->key(),N)); N += cg->dim(); @@ -158,7 +158,7 @@ pair 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 cg = bn[key]; @@ -198,7 +198,7 @@ pair matrix(const GaussianBayesNet& bn) { VectorValues rhs(const GaussianBayesNet& bn) { boost::shared_ptr result(allocateVectorValues(bn)); BOOST_FOREACH(boost::shared_ptr cg,bn) { - varid_t key = cg->key(); + Index key = cg->key(); // get sigmas const Vector& sigmas = cg->get_sigmas(); diff --git a/linear/GaussianBayesNet.h b/linear/GaussianBayesNet.h index 3389292d02..eda5a653a4 100644 --- a/linear/GaussianBayesNet.h +++ b/linear/GaussianBayesNet.h @@ -23,24 +23,24 @@ namespace gtsam { typedef BayesNet 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 diff --git a/linear/GaussianConditional.cpp b/linear/GaussianConditional.cpp index b1e2286b14..e77ad14f49 100644 --- a/linear/GaussianConditional.cpp +++ b/linear/GaussianConditional.cpp @@ -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 >& parents, const Vector& sigmas) : +GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list >& 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 >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { + for(std::list >::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(R); j = 1; - for(std::list >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { + for(std::list >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { ublas::noalias(rsd_(j)) = parent->second; ++ j; } diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index 1f1da10769..3d835ba57c 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -59,31 +59,31 @@ class GaussianConditional : public Conditional { 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 >& parents, const Vector& sigmas); + GaussianConditional(Index key, const Vector& d, + const Matrix& R, const std::list >& parents, const Vector& sigmas); /** * Constructor when matrices are already stored in a combined matrix, allows diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index dad38b89b6..cb920367ed 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -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 > &terms, +GaussianFactor::GaussianFactor(const std::vector > &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 > &t } /* ************************************************************************* */ -GaussianFactor::GaussianFactor(const std::list > &terms, +GaussianFactor::GaussianFactor(const std::list > &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 >::const_iterator term=terms.begin(); term!=terms.end(); ++term) { + for(std::list >::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 > &ter firstNonzeroBlocks_.resize(b.size(), 0); Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+terms.size()+1, b.size())); j = 0; - for(std::list >::const_iterator term=terms.begin(); term!=terms.end(); ++term) { + for(std::list >::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(jA.first,jA.second.size2())); +// result.insert(std::pair(jA.first,jA.second.size2())); // return result; //} // ///* ************************************************************************* */ -//void GaussianFactor::tally_separator(varid_t key, set& separator) const { +//void GaussianFactor::tally_separator(Index key, set& separator) const { // if(involves(key)) { // BOOST_FOREACH(const NamedMatrix& jA, As_) // if(jA.first != key) separator.insert(jA.first); @@ -336,7 +336,7 @@ pair GaussianFactor::matrix(bool weight) const { Matrix GaussianFactor::matrix_augmented(bool weight) const { // // get pointers to the matrices // vector 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&, const vector&, const vector&, const std::vector >&); -template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& factorGraph, const GaussianVariableIndex& variableIndex, const vector& factors, - const vector& variables, const std::vector >& variablePositions) { + const vector& variables, const std::vector >& variablePositions) { shared_ptr ret(boost::make_shared()); 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::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos; + std::vector::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos; std::vector::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::const_iterator keyitend = factor.keys_.end(); + std::vector::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, size_t, size_t> countDims(const std::vector::max()) { + Index sourceFactorI = 0; + BOOST_FOREACH(const Index sourceVarpos, slots.second) { + if(sourceVarpos < numeric_limits::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::max()) { + if(sourceSlot != numeric_limits::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 //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 ret = @@ -1075,7 +1075,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa //} ///* ************************************************************************* */ //pair -//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) diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index 04d18f8ed3..d47408fc0f 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -40,7 +40,7 @@ class GaussianFactorGraph; template class GaussianVariableIndex; /** A map from key to dimension, useful in various contexts */ -typedef std::map Dimensions; +typedef std::map Dimensions; /** * Base Class for a linear factor. @@ -75,24 +75,24 @@ class GaussianFactor: public Factor { 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 > &terms, + GaussianFactor(const std::vector > &terms, const Vector &b, const SharedDiagonal& model); - GaussianFactor(const std::list > &terms, + GaussianFactor(const std::list > &terms, const Vector &b, const SharedDiagonal& model); /** Construct from Conditional Gaussian */ @@ -140,7 +140,7 @@ class GaussianFactor: public Factor { template static shared_ptr Combine(const GaussianFactorGraph& factorGraph, const GaussianVariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions); + const std::vector& variables, const std::vector >& variablePositions); /** * Named constructor for combining a set of factors with pre-computed set of @@ -208,7 +208,7 @@ class GaussianFactor: public Factor { 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; diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index d4ba34434f..c3071badec 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -35,9 +35,9 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : } /* ************************************************************************* */ -std::set, boost::fast_pool_allocator > +std::set, boost::fast_pool_allocator > GaussianFactorGraph::keys() const { - std::set, boost::fast_pool_allocator > keys; + std::set, boost::fast_pool_allocator > 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 GaussianFactorGraph::find_separator(varid_t key) const +//set GaussianFactorGraph::find_separator(Index key) const //{ -// set separator; +// set 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 factors = findAndRemoveFactors(key); // // // Collect all dimensions as well as the set of separator keys -// set separator; +// set 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 frontal_set(frontals.begin(), frontals.end()); -// BOOST_FOREACH(varid_t key, this->keys()) { +// set 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 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::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 diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 9b88290325..71992dab1c 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -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 > &terms, + inline void add(const std::vector > &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, boost::fast_pool_allocator > keys() const; + std::set, boost::fast_pool_allocator > 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 find_separator(varid_t key) const; +// std::set 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& 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::fillDims(const GaussianFactorGraph& factorGraph) { // Store dimensions of each variable assert(dims_.size() == Base::index_.size()); - for(varid_t var=0; var::permute(permutation); storage_type original(this->dims_.size()); this->dims_.swap(original); - for(varid_t j=0; jdims_[j] = original[permutation[j]]; } @@ -371,7 +371,7 @@ namespace gtsam { dims_[*var] = factor->getDim(var); } } -// for(varid_t var=0; var= varIndex.dims_.size() || varIndex.dims_[var] == 0) // assert(dims_[var] != 0); diff --git a/linear/VectorMap.cpp b/linear/VectorMap.cpp index 6123c1fd6a..224ded8454 100644 --- a/linear/VectorMap.cpp +++ b/linear/VectorMap.cpp @@ -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 VectorMap::get_names() const { - std::vector names; +std::vector VectorMap::get_names() const { + std::vector 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); diff --git a/linear/VectorMap.h b/linear/VectorMap.h index 2650eb02b4..f1b1e350db 100644 --- a/linear/VectorMap.h +++ b/linear/VectorMap.h @@ -23,29 +23,29 @@ namespace gtsam { protected: /** Map from string indices to values */ - std::map values; + std::map values; public: - typedef std::map::iterator iterator; - typedef std::map::const_iterator const_iterator; + typedef std::map::iterator iterator; + typedef std::map::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 get_names() const; + std::vector 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()); } diff --git a/linear/VectorValues.h b/linear/VectorValues.h index f6d2130efc..16632b1fca 100644 --- a/linear/VectorValues.h +++ b/linear/VectorValues.h @@ -52,7 +52,7 @@ class VectorValues : public Testable { 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 @@ class VectorValues : public Testable { VectorValues(const std::vector& 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 @@ class VectorValues : public Testable { const_iterator end() const { return _impl_iterator(*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 @@ class VectorValues : public Testable { /** 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 { 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 { 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& r) { assert(&config_ == &r.config_); } friend class VectorValues; @@ -157,14 +157,14 @@ class VectorValues : public Testable { }; 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 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& 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])); diff --git a/linear/tests/testGaussianConditional.cpp b/linear/tests/testGaussianConditional.cpp index 6203d16bdb..078b31d7c3 100644 --- a/linear/tests/testGaussianConditional.cpp +++ b/linear/tests/testGaussianConditional.cpp @@ -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 > terms; + list > 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; diff --git a/linear/tests/testGaussianFactor.cpp b/linear/tests/testGaussianFactor.cpp index a84fda930a..35ee940839 100644 --- a/linear/tests/testGaussianFactor.cpp +++ b/linear/tests/testGaussianFactor.cpp @@ -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 > terms; + std::list > 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 > terms; + std::list > 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 > meas; +// vector > 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 > combinedMeasurement; +// vector > 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 > meas; + vector > 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 > terms1; + list > 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 > terms2; + list > 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 > terms3; + list > 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 > terms4; + list > 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 > cterms1; + list > 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 > cterms2; + list > 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 > cterms3; + list > 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& i) { //{ // GaussianFactor f(_x1_, eye(2), _x2_, eye(2), _l1_, eye(2), ones(2), sigma0_1); // -// std::set act1, act2, act3; +// std::set 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 > terms; +// std::list > terms; // terms.push_back(make_pair(_x0_, eye(2))); // terms.push_back(make_pair(_x1_, 2.*eye(2))); // diff --git a/linear/tests/testGaussianJunctionTree.cpp b/linear/tests/testGaussianJunctionTree.cpp index f575fbbff1..73824a3bf5 100644 --- a/linear/tests/testGaussianJunctionTree.cpp +++ b/linear/tests/testGaussianJunctionTree.cpp @@ -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() { diff --git a/linear/tests/testVectorMap.cpp b/linear/tests/testVectorMap.cpp index 5c3edc06a8..c43ec61647 100644 --- a/linear/tests/testVectorMap.cpp +++ b/linear/tests/testVectorMap.cpp @@ -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() { diff --git a/linear/tests/timeFactorOverhead.cpp b/linear/tests/timeFactorOverhead.cpp index ed92f53376..145d697f15 100644 --- a/linear/tests/timeFactorOverhead.cpp +++ b/linear/tests/timeFactorOverhead.cpp @@ -20,7 +20,7 @@ static boost::variate_generator > rg(boost int main(int argc, char *argv[]) { - varid_t key = 0; + Index key = 0; size_t vardim = 2; size_t blockdim = 1; diff --git a/linear/tests/timeGaussianFactor.cpp b/linear/tests/timeGaussianFactor.cpp index ead7e20ee9..ad5b3f95fd 100644 --- a/linear/tests/timeGaussianFactor.cpp +++ b/linear/tests/timeGaussianFactor.cpp @@ -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 diff --git a/linear/tests/timeSLAMlike.cpp b/linear/tests/timeSLAMlike.cpp index 17a556892b..12de9ffeee 100644 --- a/linear/tests/timeSLAMlike.cpp +++ b/linear/tests/timeSLAMlike.cpp @@ -21,7 +21,7 @@ using namespace boost::lambda; static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); -bool _pair_compare(const pair& a1, const pair& a2) { return a1.first < a2.first; } +bool _pair_compare(const pair& a1, const pair& 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 > terms; terms.reserve(varsPerBlock); + vector > 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 nVars-1 || find_if(terms.begin(), terms.end(), - boost::bind(&pair::first, boost::lambda::_1) == var) != terms.end()); + boost::bind(&pair::first, boost::lambda::_1) == var) != terms.end()); Matrix A(blockdim, vardim); for(size_t j=0; j - Ordering::shared_ptr LieValues::orderingArbitrary(varid_t firstVar) const { + Ordering::shared_ptr LieValues::orderingArbitrary(Index firstVar) const { // Generate an initial key ordering in iterator order _ValuesKeyOrderer keyOrderer(firstVar); this->apply(keyOrderer); diff --git a/nonlinear/LieValues.h b/nonlinear/LieValues.h index 1d097db2b2..f87009c5d9 100644 --- a/nonlinear/LieValues.h +++ b/nonlinear/LieValues.h @@ -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 dimensions; _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} template 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 void operator()(const I& key_value) { ordering->insert(key_value->first, var); ++ var; diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 38730f5c79..ce2c31d32f 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -155,7 +155,7 @@ class NonlinearConstraint1 : public NonlinearConstraint { 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 @@ class NonlinearConstraint2 : public NonlinearConstraint { 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 @@ class NonlinearConstraint2 : public NonlinearConstraint { * 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 @@ class NonlinearConstraint3 : public NonlinearConstraint { 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 @@ class NonlinearConstraint3 : public NonlinearConstraint { * 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) diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index d452816ef8..33c9760956 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -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(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(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(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) diff --git a/nonlinear/Ordering.cpp b/nonlinear/Ordering.cpp index 2aa68ee832..22b83756b1 100644 --- a/nonlinear/Ordering.cpp +++ b/nonlinear/Ordering.cpp @@ -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; } diff --git a/nonlinear/Ordering.h b/nonlinear/Ordering.h index 0559bbf66d..753af5e717 100644 --- a/nonlinear/Ordering.h +++ b/nonlinear/Ordering.h @@ -21,51 +21,51 @@ namespace gtsam { class Ordering : Testable { protected: - typedef boost::fast_pool_allocator > Allocator; - typedef std::map, Allocator> Map; + typedef boost::fast_pool_allocator > Allocator; + typedef std::map, Allocator> Map; Map order_; - varid_t nVars_; + Index nVars_; public: typedef boost::shared_ptr shared_ptr; - typedef std::pair value_type; + typedef std::pair 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 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 tryInsert(const value_type& key_order) { std::pair it_ok(order_.insert(key_order)); if(key_order.second+1 > nVars_) nVars_ = key_order.second+1; return it_ok; } - std::pair tryInsert(const Symbol& key, varid_t order) { return tryInsert(std::make_pair(key,order)); } + std::pair 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 @@ class Ordering : Testable { * @class Unordered * @brief a set of unordered indice */ -class Unordered: public std::set, public Testable { +class Unordered: public std::set, public Testable { 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& keys_in) : std::set(keys_in) {} + Unordered(const std::set& keys_in) : std::set(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; diff --git a/nonlinear/TupleValues.h b/nonlinear/TupleValues.h index 2e26d4e245..bea485888d 100644 --- a/nonlinear/TupleValues.h +++ b/nonlinear/TupleValues.h @@ -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); diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp index 544cfd3993..ccf4318643 100644 --- a/slam/GaussianISAM2.cpp +++ b/slam/GaussianISAM2.cpp @@ -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; } diff --git a/slam/LinearApproxFactor-inl.h b/slam/LinearApproxFactor-inl.h index 9518d072fc..abb10a7d31 100644 --- a/slam/LinearApproxFactor-inl.h +++ b/slam/LinearApproxFactor-inl.h @@ -22,7 +22,7 @@ LinearApproxFactor::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 LinearApproxFactor::linearize(const Values& c, const Ordering& ordering) const { // sort by varid - only known at linearization time - typedef std::map VarMatrixMap; + typedef std::map 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 > terms; + std::vector > terms; BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms) terms.push_back(p); @@ -77,7 +77,7 @@ LinearApproxFactor::linearize(const Values& c, const Ordering& order template Factor::shared_ptr LinearApproxFactor::symbolic(const Ordering& ordering) const { - std::vector key_ids(this->keys_.size()); + std::vector key_ids(this->keys_.size()); size_t i=0; BOOST_FOREACH(const Symbol& key, this->keys_) key_ids[i++] = ordering[key]; diff --git a/slam/smallExample.cpp b/slam/smallExample.cpp index ffeef61bc9..675c50b4c4 100644 --- a/slam/smallExample.cpp +++ b/slam/smallExample.cpp @@ -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 sharedNonlinearFactorGraph() { diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index c0811e9301..6460053bd3 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -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 ) diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index d03adba298..fb31f0bb40 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -43,14 +43,14 @@ TEST( GaussianJunctionTree, constructor2 ) // create an ordering GaussianJunctionTree actual(fg); - vector frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"]; - vector frontal2; frontal2 += ordering["x3"], ordering["x2"]; - vector frontal3; frontal3 += ordering["x1"]; - vector frontal4; frontal4 += ordering["x7"]; - vector sep1; - vector sep2; sep2 += ordering["x4"]; - vector sep3; sep3 += ordering["x2"]; - vector sep4; sep4 += ordering["x6"]; + vector frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"]; + vector frontal2; frontal2 += ordering["x3"], ordering["x2"]; + vector frontal3; frontal3 += ordering["x1"]; + vector frontal4; frontal4 += ordering["x7"]; + vector sep1; + vector sep2; sep2 += ordering["x4"]; + vector sep3; sep3 += ordering["x2"]; + vector sep4; sep4 += ordering["x6"]; CHECK(assert_equal(frontal1, actual.root()->frontal)); CHECK(assert_equal(sep1, actual.root()->separator)); LONGS_EQUAL(5, actual.root()->size()); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 99e07e2957..9c20e97d63 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -29,8 +29,8 @@ TEST(GaussianFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); // eliminate - list x3var; x3var.push_back(ordering["x3"]); - list x1var; x1var.push_back(ordering["x1"]); + list x3var; x3var.push_back(ordering["x3"]); + list 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 xvar; xvar.push_back(0); + list xvar; xvar.push_back(0); GaussianBayesNet actual = *Inference::Marginal(GaussianFactorGraph(cbn), xvar); // expected is just scalar Gaussian on x