Frank says: NEVER use "using namespace xxx" in a header file. Got rid of lambda.h header where not needed.

release/4.3a0
Frank Dellaert 2012-06-08 04:52:38 +00:00
parent 2ebd7a8974
commit 2885757acc
4 changed files with 109 additions and 114 deletions

View File

@ -19,57 +19,61 @@
#include <gtsam/base/Testable.h>
#include <iostream>
#include <fstream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/assign/std/vector.hpp> // for +=
using namespace boost::assign;
using namespace std;
#include <iostream>
#include <fstream>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const string& s) const {
cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
conditional->print();
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesNet<CONDITIONAL>::equals(const BayesNet& cbn, double tol) const {
if(size() != cbn.size()) return false;
return equal(conditionals_.begin(),conditionals_.end(),cbn.conditionals_.begin(),equals_star<CONDITIONAL>(tol));
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::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<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s) const {
std::cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
conditional->print();
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::find(Index key) {
for(iterator it = begin(); it != end(); ++it)
if(std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) != (*it)->endFrontals())
bool BayesNet<CONDITIONAL>::equals(const BayesNet& cbn, double tol) const {
if (size() != cbn.size())
return false;
return equal(conditionals_.begin(), conditionals_.end(),
cbn.conditionals_.begin(), equals_star<CONDITIONAL>(tol));
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::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<class CONDITIONAL>
void BayesNet<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::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<class CONDITIONAL>
void BayesNet<CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
conditional->permuteWithInverse(inversePermutation);
}
@ -77,52 +81,54 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool BayesNet<CONDITIONAL>::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<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL> bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_back(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_front(const BayesNet<CONDITIONAL> bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_front(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL> bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_back(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::popLeaf(iterator conditional) {
void BayesNet<CONDITIONAL>::push_front(const BayesNet<CONDITIONAL> bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_front(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::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<class CONDITIONAL>
FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.insert(ord.begin(), conditional->beginFrontals(), conditional->endFrontals());
return ord;
}
/* ************************************************************************* */
template<class CONDITIONAL>
FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.insert(ord.begin(), conditional->beginFrontals(),
conditional->endFrontals());
return ord;
}
// /* ************************************************************************* */
// template<class CONDITIONAL>
@ -139,21 +145,22 @@ namespace gtsam {
// of.close();
// }
//
/* ************************************************************************* */
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::sharedConditional
BayesNet<CONDITIONAL>::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<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::sharedConditional BayesNet<CONDITIONAL>::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

View File

@ -26,22 +26,16 @@
#include <gtsam/inference/inference.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <iostream>
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/format.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <fstream>
using namespace boost::assign;
namespace lam = boost::lambda;
#include <iostream>
#include <algorithm>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
@ -64,8 +58,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::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<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s, sharedClique clique, int parentnum) const {
void BayesTree<CONDITIONAL,CLIQUE>::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<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::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<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, list<sharedClique>& child_cliques) {
const sharedConditional& conditional, std::list<sharedClique>& 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<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > 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<sharedClique> childRoots;
std::list<sharedClique> childRoots;
typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end());
@ -329,12 +323,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::print(const string& s) const {
void BayesTree<CONDITIONAL,CLIQUE>::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<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
const sharedConditional& clique, list<sharedClique>& children, bool isRootClique) {
const sharedConditional& clique, std::list<sharedClique>& 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<FactorType> p_C1C2(C1->joint(C2, root_, function));
// eliminate remaining factor graph to get requested joint
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
std::vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
GenericSequentialSolver<FactorType> solver(p_C1C2);
return solver.jointFactorGraph(key12,function);
}

View File

@ -24,14 +24,12 @@
#include <gtsam/inference/IndexConditional.h>
#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/static_assert.hpp>
#include <iostream>
#include <set>
#include <vector>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
@ -41,7 +39,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::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<FACTOR> factors;
@ -60,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::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<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
/* ************************************************************************* */
template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
std::vector<Index> EliminationTree<FACTOR>::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<Index>::max();
static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns
vector<Index> parents(n, none);
vector<Index> prevCol(m, none);
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do
for (Index j = 0; j < n; j++) {
@ -111,17 +109,17 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
tic(1, "ET ComputeParents");
// Compute the tree structure
vector<Index> parents(ComputeParents(structure));
std::vector<Index> parents(ComputeParents(structure));
toc(1, "ET ComputeParents");
// Number of variables
const size_t n = structure.size();
static const Index none = numeric_limits<Index>::max();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
tic(2, "assemble tree");
vector<shared_ptr> trees(n);
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationTree(j)); // Create a new node on this variable
@ -169,7 +167,7 @@ EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
/* ************************************************************************* */
template<class FACTORGRAPH>
void EliminationTree<FACTORGRAPH>::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_) {

View File

@ -19,19 +19,15 @@
#pragma once
#include <gtsam/base/timing.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {