diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 53e0ec18d..88b386b72 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -19,57 +19,61 @@ #include -#include -#include #include #include #include -#include -#include #include // for += using namespace boost::assign; -using namespace std; +#include +#include namespace gtsam { - /* ************************************************************************* */ - template - void BayesNet::print(const string& s) const { - cout << s; - BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_) - conditional->print(); - } - - /* ************************************************************************* */ - template - bool BayesNet::equals(const BayesNet& cbn, double tol) const { - if(size() != cbn.size()) return false; - return equal(conditionals_.begin(),conditionals_.end(),cbn.conditionals_.begin(),equals_star(tol)); - } - /* ************************************************************************* */ - template - typename BayesNet::const_iterator BayesNet::find(Index key) const { - for(const_iterator it = begin(); it != end(); ++it) - if(std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) != (*it)->endFrontals()) - return it; - return end(); - } + template + void BayesNet::print(const std::string& s) const { + std::cout << s; + BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_) + conditional->print(); + } /* ************************************************************************* */ template - typename BayesNet::iterator BayesNet::find(Index key) { - for(iterator it = begin(); it != end(); ++it) - if(std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) != (*it)->endFrontals()) + bool BayesNet::equals(const BayesNet& cbn, double tol) const { + if (size() != cbn.size()) + return false; + return equal(conditionals_.begin(), conditionals_.end(), + cbn.conditionals_.begin(), equals_star(tol)); + } + + /* ************************************************************************* */ + template + typename BayesNet::const_iterator BayesNet::find( + Index key) const { + for (const_iterator it = begin(); it != end(); ++it) + if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) + != (*it)->endFrontals()) return it; return end(); } /* ************************************************************************* */ template - void BayesNet::permuteWithInverse(const Permutation& inversePermutation) { + typename BayesNet::iterator BayesNet::find( + Index key) { + for (iterator it = begin(); it != end(); ++it) + if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) + != (*it)->endFrontals()) + return it; + return end(); + } + + /* ************************************************************************* */ + template + void BayesNet::permuteWithInverse( + const Permutation& inversePermutation) { BOOST_FOREACH(sharedConditional conditional, conditionals_) { conditional->permuteWithInverse(inversePermutation); } @@ -77,52 +81,54 @@ namespace gtsam { /* ************************************************************************* */ template - bool BayesNet::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool BayesNet::permuteSeparatorWithInverse( + const Permutation& inversePermutation) { bool separatorChanged = false; BOOST_FOREACH(sharedConditional conditional, conditionals_) { - if(conditional->permuteSeparatorWithInverse(inversePermutation)) + if (conditional->permuteSeparatorWithInverse(inversePermutation)) separatorChanged = true; } return separatorChanged; } - /* ************************************************************************* */ - template - void BayesNet::push_back(const BayesNet bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_back(conditional); - } - - /* ************************************************************************* */ - template - void BayesNet::push_front(const BayesNet bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_front(conditional); - } + /* ************************************************************************* */ + template + void BayesNet::push_back(const BayesNet bn) { + BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) + push_back(conditional); + } /* ************************************************************************* */ template - void BayesNet::popLeaf(iterator conditional) { + void BayesNet::push_front(const BayesNet bn) { + BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) + push_front(conditional); + } + + /* ************************************************************************* */ + template + void BayesNet::popLeaf(iterator conditional) { #ifndef NDEBUG BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) { BOOST_FOREACH(Index key, (*conditional)->frontals()) { if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents()) - throw std::invalid_argument( - "Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf."); + throw std::invalid_argument( + "Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf."); } } #endif conditionals_.erase(conditional); - } + } - /* ************************************************************************* */ - template - FastList BayesNet::ordering() const { - FastList ord; - BOOST_FOREACH(sharedConditional conditional,conditionals_) - ord.insert(ord.begin(), conditional->beginFrontals(), conditional->endFrontals()); - return ord; - } + /* ************************************************************************* */ + template + FastList BayesNet::ordering() const { + FastList ord; + BOOST_FOREACH(sharedConditional conditional,conditionals_) + ord.insert(ord.begin(), conditional->beginFrontals(), + conditional->endFrontals()); + return ord; + } // /* ************************************************************************* */ // template @@ -139,21 +145,22 @@ namespace gtsam { // of.close(); // } // - /* ************************************************************************* */ + /* ************************************************************************* */ - template - typename BayesNet::sharedConditional - BayesNet::operator[](Index key) const { - BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::const_iterator it = std::find(conditional->beginFrontals(), conditional->endFrontals(), key); - if (it!=conditional->endFrontals()) { + template + typename BayesNet::sharedConditional BayesNet::operator[]( + Index key) const { + BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { + typename CONDITIONAL::const_iterator it = std::find( + conditional->beginFrontals(), conditional->endFrontals(), key); + if (it != conditional->endFrontals()) { return conditional; } - } - throw(invalid_argument((boost::format( - "BayesNet::operator['%1%']: not found") % key).str())); - } + } + throw(std::invalid_argument( + (boost::format("BayesNet::operator['%1%']: not found") % key).str())); + } - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 0aefeaa19..0100bdd73 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -26,22 +26,16 @@ #include #include -#include -#include - #include #include // for operator += #include -#include -#include + #include -using namespace boost::assign; -namespace lam = boost::lambda; +#include +#include namespace gtsam { - using namespace std; - /* ************************************************************************* */ template typename BayesTree::CliqueData @@ -64,8 +58,8 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::saveGraph(const std::string &s) const { - if (!root_.get()) throw invalid_argument("the root of Bayes tree has not been initialized!"); - ofstream of(s.c_str()); + if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); + std::ofstream of(s.c_str()); of<< "digraph G{\n"; saveGraph(of, root_); of<<"}"; @@ -73,12 +67,12 @@ namespace gtsam { } template - void BayesTree::saveGraph(ostream &s, sharedClique clique, int parentnum) const { + void BayesTree::saveGraph(std::ostream &s, sharedClique clique, int parentnum) const { static int num = 0; bool first = true; std::stringstream out; out << num; - string parent = out.str(); + std::string parent = out.str(); parent += "[label=\""; BOOST_FOREACH(Index index, clique->conditional_->frontals()) { @@ -136,7 +130,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::Cliques::print(const std::string& s) const { - cout << s << ":\n"; + std::cout << s << ":\n"; BOOST_FOREACH(sharedClique clique, *this) clique->printTree(); } @@ -175,7 +169,7 @@ namespace gtsam { /* ************************************************************************* */ template typename BayesTree::sharedClique BayesTree::addClique( - const sharedConditional& conditional, list& child_cliques) { + const sharedConditional& conditional, std::list& child_cliques) { sharedClique new_clique(new Clique(conditional)); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); BOOST_FOREACH(Index key, conditional->frontals()) @@ -290,10 +284,10 @@ namespace gtsam { template BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { if (bayesNet.size() == 0) - throw invalid_argument("BayesTree::insert: empty bayes net!"); + throw std::invalid_argument("BayesTree::insert: empty bayes net!"); // get the roots of child subtrees and merge their nodes_ - list childRoots; + std::list childRoots; typedef BayesTree Tree; BOOST_FOREACH(const Tree& subtree, subtrees) { nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end()); @@ -329,12 +323,12 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::print(const string& s) const { + void BayesTree::print(const std::string& s) const { if (root_.use_count() == 0) { printf("WARNING: BayesTree.print encountered a forest...\n"); return; } - cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << endl; + std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl; if (nodes_.empty()) return; root_->printTree(""); } @@ -379,7 +373,7 @@ namespace gtsam { // if no parents, start a new root clique if (parents.empty()) { - if(debug) cout << "No parents so making root" << endl; + if(debug) std::cout << "No parents so making root" << std::endl; bayesTree.root_ = bayesTree.addClique(conditional); return; } @@ -387,13 +381,13 @@ namespace gtsam { // otherwise, find the parent clique by using the index data structure // to find the lowest-ordered parent Index parentRepresentative = bayesTree.findParentClique(parents); - if(debug) cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << endl; + if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; sharedClique parent_clique = bayesTree[parentRepresentative]; if(debug) parent_clique->print("Parent clique is "); // if the parents and parent clique have the same size, add to parent clique if ((*parent_clique)->size() == size_t(parents.size())) { - if(debug) cout << "Adding to parent clique" << endl; + if(debug) std::cout << "Adding to parent clique" << std::endl; #ifndef NDEBUG // Debug check that the parent keys of the new conditional match the keys // currently in the clique. @@ -408,7 +402,7 @@ namespace gtsam { #endif addToCliqueFront(bayesTree, conditional, parent_clique); } else { - if(debug) cout << "Starting new clique" << endl; + if(debug) std::cout << "Starting new clique" << std::endl; // otherwise, start a new clique and add it to the tree bayesTree.addClique(conditional,parent_clique); } @@ -418,10 +412,10 @@ namespace gtsam { //TODO: remove this function after removing TSAM.cpp template typename BayesTree::sharedClique BayesTree::insert( - const sharedConditional& clique, list& children, bool isRootClique) { + const sharedConditional& clique, std::list& children, bool isRootClique) { if (clique->nrFrontals() == 0) - throw invalid_argument("BayesTree::insert: empty clique!"); + throw std::invalid_argument("BayesTree::insert: empty clique!"); // create a new clique and add all the conditionals to the clique sharedClique new_clique = addClique(clique, children); @@ -512,7 +506,7 @@ namespace gtsam { FactorGraph p_C1C2(C1->joint(C2, root_, function)); // eliminate remaining factor graph to get requested joint - vector key12(2); key12[0] = key1; key12[1] = key2; + std::vector key12(2); key12[0] = key1; key12[1] = key2; GenericSequentialSolver solver(p_C1C2); return solver.jointFactorGraph(key12,function); } diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index e5f50a867..2f62f79af 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -24,14 +24,12 @@ #include #include -#include #include + #include #include #include -using namespace std; - namespace gtsam { /* ************************************************************************* */ @@ -41,7 +39,7 @@ typename EliminationTree::sharedFactor EliminationTree::eliminat static const bool debug = false; - if(debug) cout << "ETree: eliminating " << this->key_ << endl; + if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; // Create the list of factors to be eliminated, initially empty, and reserve space FactorGraph factors; @@ -60,7 +58,7 @@ typename EliminationTree::sharedFactor EliminationTree::eliminat eliminated(function(factors, 1)); conditionals[this->key_] = eliminated.first; - if(debug) cout << "Eliminated " << this->key_ << " to get:\n"; + if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n"; if(debug) eliminated.first->print("Conditional: "); if(debug) eliminated.second->print("Factor: "); @@ -69,17 +67,17 @@ typename EliminationTree::sharedFactor EliminationTree::eliminat /* ************************************************************************* */ template -vector EliminationTree::ComputeParents(const VariableIndex& structure) { +std::vector EliminationTree::ComputeParents(const VariableIndex& structure) { // Number of factors and variables const size_t m = structure.nFactors(); const size_t n = structure.size(); - static const Index none = numeric_limits::max(); + static const Index none = std::numeric_limits::max(); // Allocate result parent vector and vector of last factor columns - vector parents(n, none); - vector prevCol(m, none); + std::vector parents(n, none); + std::vector prevCol(m, none); // for column j \in 1 to n do for (Index j = 0; j < n; j++) { @@ -111,17 +109,17 @@ typename EliminationTree::shared_ptr EliminationTree::Create( tic(1, "ET ComputeParents"); // Compute the tree structure - vector parents(ComputeParents(structure)); + std::vector parents(ComputeParents(structure)); toc(1, "ET ComputeParents"); // Number of variables const size_t n = structure.size(); - static const Index none = numeric_limits::max(); + static const Index none = std::numeric_limits::max(); // Create tree structure tic(2, "assemble tree"); - vector trees(n); + std::vector trees(n); for (Index k = 1; k <= n; k++) { Index j = n - k; // Start at the last variable and loop down to 0 trees[j].reset(new EliminationTree(j)); // Create a new node on this variable @@ -169,7 +167,7 @@ EliminationTree::Create(const FactorGraph& factorGraph) { /* ************************************************************************* */ template void EliminationTree::print(const std::string& name) const { - cout << name << " (" << key_ << ")" << endl; + std::cout << name << " (" << key_ << ")" << std::endl; BOOST_FOREACH(const sharedFactor& factor, factors_) { factor->print(name + " "); } BOOST_FOREACH(const shared_ptr& child, subTrees_) { diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index bf1c17890..c1add38bf 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -19,19 +19,15 @@ #pragma once -#include #include #include #include +#include #include -#include -#include namespace gtsam { - using namespace std; - /* ************************************************************************* */ template void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) {