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

Changed a few templates and typedefs to FACTOR and Factor

release/4.3a0
Richard Roberts 2010-10-17 17:10:10 +00:00
parent 4a7b8bad27
commit d94fa41f8a
10 changed files with 54 additions and 392 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
inference/EliminationTree-inl.h
View File

@ -42,7 +42,7 @@ EliminationTree<FACTORGRAPH>::eliminate_() const {
} }
// eliminate the joint factor and add the conditional to the bayes net // eliminate the joint factor and add the conditional to the bayes net
typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::factor_type::Combine(factors, VariableSlots(factors))); typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::Factor::Combine(factors, VariableSlots(factors)));
bayesNet.push_back(jointFactor->eliminateFirst()); bayesNet.push_back(jointFactor->eliminateFirst());
return EliminationResult(bayesNet, jointFactor); return EliminationResult(bayesNet, jointFactor);

2
inference/EliminationTree.h
View File

@ -25,7 +25,7 @@ class EliminationTree: public Testable<EliminationTree<FACTORGRAPH> > {
public: public:
typedef boost::shared_ptr<typename FACTORGRAPH::factor_type> sharedFactor; typedef boost::shared_ptr<typename FACTORGRAPH::Factor> sharedFactor;
typedef boost::shared_ptr<EliminationTree<FACTORGRAPH> > shared_ptr; typedef boost::shared_ptr<EliminationTree<FACTORGRAPH> > shared_ptr;
typedef FACTORGRAPH FactorGraph; typedef FACTORGRAPH FactorGraph;

292
inference/FactorGraph-inl.h
View File

@ -40,29 +40,23 @@
#define INSTANTIATE_FACTOR_GRAPH(F) \ #define INSTANTIATE_FACTOR_GRAPH(F) \
template class FactorGraph<F>; \ template class FactorGraph<F>; \
/*template boost::shared_ptr<F> removeAndCombineFactors(FactorGraph<F>&, const std::string&);*/ \
template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&); template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&);
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
// /* ************************************************************************* */
// template<class Conditional>
// FactorGraph::FactorGraph(const BayesNet<Conditional>& bayesNet, const Inference::Permutation& permutation) {
// }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) { void FactorGraph<FACTOR>::push_back(const FactorGraph<FACTOR>& factors) {
const_iterator factor = factors.begin(); const_iterator factor = factors.begin();
for (; factor != factors.end(); factor++) for (; factor != factors.end(); factor++)
push_back(*factor); push_back(*factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
void FactorGraph<Factor>::print(const string& s) const { void FactorGraph<FACTOR>::print(const string& s) const {
cout << s << endl; cout << s << endl;
printf("size: %d\n", (int) size()); printf("size: %d\n", (int) size());
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
@ -73,8 +67,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
bool FactorGraph<Factor>::equals(const FactorGraph<Factor>& fg, double tol) const { bool FactorGraph<FACTOR>::equals(const FactorGraph<FACTOR>& fg, double tol) const {
/** check whether the two factor graphs have the same number of factors_ */ /** check whether the two factor graphs have the same number of factors_ */
if (factors_.size() != fg.size()) return false; if (factors_.size() != fg.size()) return false;
@ -90,232 +84,17 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
size_t FactorGraph<Factor>::nrFactors() const { size_t FactorGraph<FACTOR>::nrFactors() const {
size_t size_ = 0; size_t size_ = 0;
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
if (*factor != NULL) size_++; if (*factor != NULL) size_++;
return size_; return size_;
} }
// /* ************************************************************************* *
// * Call colamd given a column-major symbolic matrix A
// * @param n_col colamd arg 1: number of rows in A
// * @param n_row colamd arg 2: number of columns in A
// * @param nrNonZeros number of non-zero entries in A
// * @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<Index, vector<int> >& columns,
// Ordering& ordering, const set<Index>& lastKeys) {
//
// // Convert to compressed column major format colamd wants it in (== MATLAB format!)
// int Alen = ccolamd_recommended(nrNonZeros, n_row, n_col); /* colamd arg 3: size of the array A */
// int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */
// int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
// int * cmember = new int[n_col]; /* Constraint set of A, of size n_col */
//
// p[0] = 0;
// int j = 1;
// int count = 0;
// typedef map<Index, vector<int> >::const_iterator iterator;
// bool front_exists = false;
// vector<Index> initialOrder;
// for (iterator it = columns.begin(); it != columns.end(); it++) {
// Index key = it->first;
// const vector<int>& column = it->second;
// initialOrder.push_back(key);
// BOOST_FOREACH(int i, column)
// A[count++] = i; // copy sparse column
// p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1
// if (lastKeys.find(key) == lastKeys.end()) {
// cmember[j - 1] = 0;
// front_exists = true;
// } else {
// cmember[j - 1] = 1; // force lastKeys to be at the end
// }
// j += 1;
// }
// if (!front_exists) { // if only 1 entries, set everything to 0...
// for (int j = 0; j < n_col; j++)
// cmember[j] = 0;
// }
//
// double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
// int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
//
// // call colamd, result will be in p *************************************************
// /* TODO: returns (1) if successful, (0) otherwise*/
// ::ccolamd(n_row, n_col, Alen, A, p, knobs, stats, cmember);
// // **********************************************************************************
// delete[] A; // delete symbolic A
// delete[] cmember;
//
// // Convert elimination ordering in p to an ordering
// for (int j = 0; j < n_col; j++)
// ordering.push_back(initialOrder[p[j]]);
// delete[] p; // delete colamd result vector
// }
//
// /* ************************************************************************* */
// template<class Factor>
// void FactorGraph<Factor>::getOrdering(Ordering& ordering,
// const set<Index>& lastKeys,
// boost::optional<const set<Index>&> scope) const {
//
// // A factor graph is really laid out in row-major format, each factor a row
// // Below, we compute a symbolic matrix stored in sparse columns.
// map<Index, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
// int nrNonZeros = 0; // number of non-zero entries
// int n_row = 0; /* colamd arg 1: number of rows in A */
//
// // loop over all factors = rows
// bool inserted;
// bool hasInterested = scope.is_initialized();
// BOOST_FOREACH(const sharedFactor& factor, factors_) {
// if (factor == NULL) continue;
// const vector<Index>& keys(factor->keys());
// inserted = false;
// BOOST_FOREACH(Index key, keys) {
// if (!hasInterested || scope->find(key) != scope->end()) {
// columns[key].push_back(n_row);
// nrNonZeros++;
// inserted = true;
// }
// }
// if (inserted) n_row++;
// }
// int n_col = (int) (columns.size()); /* colamd arg 2: number of columns in A */
// if (n_col != 0) colamd(n_col, n_row, nrNonZeros, columns, ordering, lastKeys);
// }
//
// /* ************************************************************************* */
// template<class Factor>
// Ordering FactorGraph<Factor>::getOrdering() const {
// Ordering ordering;
// set<Index> lastKeys;
// getOrdering(ordering, lastKeys);
// return ordering;
// }
//
// /* ************************************************************************* */
// template<class Factor>
// boost::shared_ptr<Ordering> FactorGraph<Factor>::getOrdering_() const {
// boost::shared_ptr<Ordering> ordering(new Ordering);
// set<Index> lastKeys;
// getOrdering(*ordering, lastKeys);
// return ordering;
// }
//
// /* ************************************************************************* */
// template<class Factor>
// Ordering FactorGraph<Factor>::getOrdering(const set<Index>& scope) const {
// Ordering ordering;
// set<Index> lastKeys;
// getOrdering(ordering, lastKeys, scope);
// return ordering;
// }
//
// /* ************************************************************************* */
// template<class Factor>
// Ordering FactorGraph<Factor>::getConstrainedOrdering(
// const set<Index>& lastKeys) const {
// Ordering ordering;
// getOrdering(ordering, lastKeys);
// return ordering;
// }
// /* ************************************************************************* */
// template<class Factor> template<class Key, class Factor2>
// PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
//
// SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor> , Factor2, Key>(
// *this);
//
// // find minimum spanning tree
// vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
// prim_minimum_spanning_tree(g, &p_map[0]);
//
// // convert edge to string pairs
// PredecessorMap<Key> tree;
// typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
// typename vector<typename SDGraph<Key>::Vertex>::iterator vi;
// for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
// Key key = boost::get(boost::vertex_name, g, *itVertex);
// Key parent = boost::get(boost::vertex_name, g, *vi);
// tree.insert(key, parent);
// }
//
// return tree;
// }
//
// /* ************************************************************************* */
// template<class Factor> template<class Key, class Factor2>
// void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree,
// FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const {
//
// BOOST_FOREACH(const sharedFactor& factor, factors_)
// {
// if (factor->keys().size() > 2) throw(invalid_argument(
// "split: only support factors with at most two keys"));
//
// if (factor->keys().size() == 1) {
// Ab1.push_back(factor);
// continue;
// }
//
// boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast<
// Factor2>(factor);
// if (!factor2) continue;
//
// Key key1 = factor2->key1();
// Key key2 = factor2->key2();
// // if the tree contains the key
// if ((tree.find(key1) != tree.end()
// && tree.find(key1)->second.compare(key2) == 0) || (tree.find(
// key2) != tree.end() && tree.find(key2)->second.compare(key1)
// == 0))
// Ab1.push_back(factor2);
// else
// Ab2.push_back(factor2);
// }
// }
// /* ************************************************************************* */
// template<class Factor>
// std::pair<FactorGraph<Factor> , FactorGraph<Factor> > FactorGraph<Factor>::splitMinimumSpanningTree() const {
// // create an empty factor graph T (tree) and factor graph C (constraints)
// FactorGraph<Factor> T;
// FactorGraph<Factor> C;
// DSF<Symbol> dsf(keys());
//
// // while G is nonempty and T is not yet spanning
// for (size_t i = 0; i < size(); i++) {
// const sharedFactor& f = factors_[i];
//
// // retrieve the labels of all the keys
// set<Symbol> labels;
// BOOST_FOREACH(const Symbol& key, f->keys())
// labels.insert(dsf.findSet(key));
//
// // if that factor connects two different trees, then add it to T
// if (labels.size() > 1) {
// T.push_back(f);
// set<Symbol>::const_iterator it = labels.begin();
// Symbol root = *it;
// for (it++; it != labels.end(); it++)
// dsf = dsf.makeUnion(root, *it);
// } else
// // otherwise add that factor to C
// C.push_back(f);
// }
// return make_pair(T, C);
// }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
void FactorGraph<Factor>::replace(size_t index, sharedFactor factor) { void FactorGraph<FACTOR>::replace(size_t index, sharedFactor factor) {
if (index >= factors_.size()) throw invalid_argument(boost::str( if (index >= factors_.size()) throw invalid_argument(boost::str(
boost::format("Factor graph does not contain a factor with index %d.") boost::format("Factor graph does not contain a factor with index %d.")
% index)); % index));
@ -323,43 +102,6 @@ namespace gtsam {
factors_[index] = factor; factors_[index] = factor;
} }
// /* ************************************************************************* */
// template<class Factor>
// std::pair<FactorGraph<Factor> , set<Index> > FactorGraph<Factor>::removeSingletons() {
// FactorGraph<Factor> singletonGraph;
// set<Index> singletons;
//
// while (true) {
// // find all the singleton variables
// Ordering new_singletons;
// Index key;
// list<size_t> indices;
// BOOST_FOREACH(boost::tie(key, indices), indices_)
// {
// // find out the number of factors associated with the current key
// size_t numValidFactors = 0;
// BOOST_FOREACH(const size_t& i, indices)
// if (factors_[i] != NULL) numValidFactors++;
//
// if (numValidFactors == 1) {
// new_singletons.push_back(key);
// BOOST_FOREACH(const size_t& i, indices)
// if (factors_[i] != NULL) singletonGraph.push_back(
// factors_[i]);
// }
// }
// singletons.insert(new_singletons.begin(), new_singletons.end());
//
// BOOST_FOREACH(const Index& singleton, new_singletons)
// findAndRemoveFactors(singleton);
//
// // exit when there are no more singletons
// if (new_singletons.empty()) break;
// }
//
// return make_pair(singletonGraph, singletons);
// }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FactorGraph>
FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) {
@ -372,19 +114,5 @@ namespace gtsam {
return fg; return fg;
} }
// /* ************************************************************************* */
// template<class Factor> boost::shared_ptr<Factor> removeAndCombineFactors(
// FactorGraph<Factor>& factorGraph, const Index& key) {
//
// // find and remove the factors associated with key
// vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
//
// // make a vector out of them and create a new factor
// boost::shared_ptr<Factor> new_factor(new Factor(found));
//
// // return it
// return new_factor;
// }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

120
inference/FactorGraph.h
View File

@ -23,10 +23,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
//#include <boost/serialization/map.hpp> #include <boost/serialization/nvp.hpp>
//#include <boost/serialization/list.hpp>
//#include <boost/serialization/vector.hpp>
//#include <boost/serialization/shared_ptr.hpp>
#include <boost/pool/pool_alloc.hpp> #include <boost/pool/pool_alloc.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
@ -42,12 +39,12 @@ namespace gtsam {
* *
* Templated on the type of factors and values structure. * Templated on the type of factors and values structure.
*/ */
template<class Factor> template<class FACTOR>
class FactorGraph: public Testable<FactorGraph<Factor> > { class FactorGraph: public Testable<FactorGraph<FACTOR> > {
public: public:
typedef Factor factor_type; typedef FACTOR Factor;
typedef boost::shared_ptr<FactorGraph<Factor> > shared_ptr; typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
typedef typename boost::shared_ptr<Factor> sharedFactor; typedef typename boost::shared_ptr<FACTOR> sharedFactor;
typedef typename std::vector<sharedFactor>::iterator iterator; typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator; typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
@ -56,13 +53,6 @@ namespace gtsam {
/** Collection of factors */ /** Collection of factors */
std::vector<sharedFactor> factors_; std::vector<sharedFactor> factors_;
// /**
// * 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<Index>& lastKeys,
// boost::optional<const std::set<Index>&> scope = boost::none) const;
public: public:
/** ------------------ Creating Factor Graphs ---------------------------- */ /** ------------------ Creating Factor Graphs ---------------------------- */
@ -83,7 +73,7 @@ namespace gtsam {
void push_back(const boost::shared_ptr<DerivedFactor>& factor); void push_back(const boost::shared_ptr<DerivedFactor>& factor);
/** push back many factors */ /** push back many factors */
void push_back(const FactorGraph<Factor>& factors); void push_back(const FactorGraph<FACTOR>& factors);
/** push back many factors with an iterator */ /** push back many factors with an iterator */
template<typename Iterator> template<typename Iterator>
@ -101,65 +91,29 @@ namespace gtsam {
operator const std::vector<sharedFactor>&() const { return factors_; } operator const std::vector<sharedFactor>&() const { return factors_; }
/** STL begin and end, so we can use BOOST_FOREACH */ /** STL begin and end, so we can use BOOST_FOREACH */
inline const_iterator begin() const { return factors_.begin();} const_iterator begin() const { return factors_.begin();}
inline const_iterator end() const { return factors_.end(); } const_iterator end() const { return factors_.end(); }
/** Get a specific factor by index */ /** Get a specific factor by index */
inline sharedFactor operator[](size_t i) const { assert(i<factors_.size()); return factors_[i]; } sharedFactor operator[](size_t i) const { assert(i<factors_.size()); return factors_[i]; }
/** Get the first factor */ /** Get the first factor */
inline sharedFactor front() const { return factors_.front(); } sharedFactor front() const { return factors_.front(); }
/** Get the last factor */ /** Get the last factor */
inline sharedFactor back() const { return factors_.back(); } sharedFactor back() const { return factors_.back(); }
/** return the number of factors and NULLS */ /** return the number of factors and NULLS */
inline size_t size() const { return factors_.size();} size_t size() const { return factors_.size();}
/** return the number valid factors */ /** return the number valid factors */
size_t nrFactors() const; size_t nrFactors() const;
// /** return keys in some random order */
// Ordering keys() const;
// /**
// * Compute colamd ordering, including I/O, constrained ordering,
// * and shared pointer version.
// */
// Ordering getOrdering() const;
// boost::shared_ptr<Ordering> getOrdering_() const;
// Ordering getOrdering(const std::set<Index>& scope) const;
// Ordering getConstrainedOrdering(const std::set<Index>& lastKeys) const;
// /**
// * find the minimum spanning tree using boost graph library
// */
// template<class Key, class Factor2>
// PredecessorMap<Key> findMinimumSpanningTree() const;
//
// /**
// * Split the graph into two parts: one corresponds to the given spanning tree,
// * and the other corresponds to the rest of the factors
// */
// template<class Key, class Factor2>
// void split(const PredecessorMap<Key>& tree, FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
// /**
// * find the minimum spanning tree using DSF
// */
// std::pair<FactorGraph<Factor> , FactorGraph<Factor> >
// SL-NEEDED? splitMinimumSpanningTree() const;
// /**
// * Check consistency of the index map, useful for debugging
// */
// void checkGraphConsistency() const;
/** ----------------- Modifying Factor Graphs ---------------------------- */ /** ----------------- Modifying Factor Graphs ---------------------------- */
/** STL begin and end, so we can use BOOST_FOREACH */ /** STL begin and end, so we can use BOOST_FOREACH */
inline iterator begin() { return factors_.begin();} iterator begin() { return factors_.begin();}
inline iterator end() { return factors_.end(); } iterator end() { return factors_.end(); }
/** /**
* Reserve space for the specified number of factors if you know in * Reserve space for the specified number of factors if you know in
@ -173,16 +127,6 @@ namespace gtsam {
/** replace a factor by index */ /** replace a factor by index */
void replace(size_t index, sharedFactor factor); void replace(size_t index, sharedFactor factor);
// /**
// * Find all the factors that involve the given node and remove them
// * from the factor graph
// * @param key the key for the given node
// */
// std::vector<sharedFactor> findAndRemoveFactors(Index key);
//
// /** remove singleton variables and the related factors */
// std::pair<FactorGraph<Factor>, std::set<Index> > removeSingletons();
private: private:
/** Serialization function */ /** Serialization function */
@ -202,16 +146,6 @@ namespace gtsam {
template<class FactorGraph> template<class FactorGraph>
FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2); FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2);
// /**
// * Extract and combine all the factors that involve a given node
// * Put this here as not all Factors have a combine constructor
// * @param key the key for the given node
// * @return the combined linear factor
// */
// template<class Factor> boost::shared_ptr<Factor>
// removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Index& key);
/** /**
* These functions are defined here because they are templated on an * These functions are defined here because they are templated on an
* additional parameter. Putting them in the -inl.h file would mean these * additional parameter. Putting them in the -inl.h file would mean these
@ -220,43 +154,43 @@ namespace gtsam {
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
template<class DerivedFactor> template<class DerivedFactor>
FactorGraph<Factor>::FactorGraph(const FactorGraph<DerivedFactor>& factorGraph) { FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DerivedFactor>& factorGraph) {
factors_.reserve(factorGraph.size()); factors_.reserve(factorGraph.size());
BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) { BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) {
if(factor) if(factor)
this->push_back(sharedFactor(new Factor(*factor))); this->push_back(sharedFactor(new FACTOR(*factor)));
else else
this->push_back(sharedFactor()); this->push_back(sharedFactor());
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
template<class Conditional> template<class Conditional>
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) { FactorGraph<FACTOR>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
factors_.reserve(bayesNet.size()); factors_.reserve(bayesNet.size());
BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) { BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) {
this->push_back(sharedFactor(new Factor(*cond))); this->push_back(sharedFactor(new FACTOR(*cond)));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
template<class DerivedFactor> template<class DerivedFactor>
inline void FactorGraph<Factor>::push_back(const boost::shared_ptr<DerivedFactor>& factor) { inline void FactorGraph<FACTOR>::push_back(const boost::shared_ptr<DerivedFactor>& factor) {
#ifndef NDEBUG #ifndef NDEBUG
factors_.push_back(boost::dynamic_pointer_cast<Factor>(factor)); // add the actual factor factors_.push_back(boost::dynamic_pointer_cast<FACTOR>(factor)); // add the actual factor
#else #else
factors_.push_back(boost::static_pointer_cast<Factor>(factor)); factors_.push_back(boost::static_pointer_cast<FACTOR>(factor));
#endif #endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
template<typename Iterator> template<typename Iterator>
void FactorGraph<Factor>::push_back(Iterator firstFactor, Iterator lastFactor) { void FactorGraph<FACTOR>::push_back(Iterator firstFactor, Iterator lastFactor) {
Iterator factor = firstFactor; Iterator factor = firstFactor;
while(factor != lastFactor) while(factor != lastFactor)
this->push_back(*(factor++)); this->push_back(*(factor++));

4
inference/JunctionTree-inl.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
Index maxVar = 0; Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i) for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) { if(fg[i]) {
typename FG::factor_type::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end()); typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min == fg[i]->end()) if(min == fg[i]->end())
lowestOrdered[i] = numeric_limits<Index>::max(); lowestOrdered[i] = numeric_limits<Index>::max();
else { else {
@ -168,7 +168,7 @@ namespace gtsam {
// Now that we know which factors and variables, and where variables // Now that we know which factors and variables, and where variables
// come from and go to, create and eliminate the new joint factor. // come from and go to, create and eliminate the new joint factor.
tic("JT 2.2 Combine"); tic("JT 2.2 Combine");
typename FG::sharedFactor jointFactor = FG::factor_type::Combine(fg, variableSlots); typename FG::sharedFactor jointFactor = FG::Factor::Combine(fg, variableSlots);
toc("JT 2.2 Combine"); toc("JT 2.2 Combine");
tic("JT 2.3 Eliminate"); tic("JT 2.3 Eliminate");
typename FG::bayesnet_type::shared_ptr fragment = jointFactor->eliminate(current->frontal.size()); typename FG::bayesnet_type::shared_ptr fragment = jointFactor->eliminate(current->frontal.size());

2
inference/JunctionTree.h
View File

@ -46,7 +46,7 @@ namespace gtsam {
typedef typename ClusterTree<FG>::Cluster Clique; typedef typename ClusterTree<FG>::Cluster Clique;
typedef typename Clique::shared_ptr sharedClique; typedef typename Clique::shared_ptr sharedClique;
typedef class BayesTree<typename FG::factor_type::Conditional> BayesTree; typedef class BayesTree<typename FG::Factor::Conditional> BayesTree;
// And we will frequently refer to a symbolic Bayes tree // And we will frequently refer to a symbolic Bayes tree
typedef gtsam::BayesTree<Conditional> SymbolicBayesTree; typedef gtsam::BayesTree<Conditional> SymbolicBayesTree;

8
inference/SymbolicFactorGraph.h
View File

@ -57,8 +57,8 @@ public:
/** /**
* Construct from a factor graph of any type * Construct from a factor graph of any type
*/ */
template<class Factor> template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<Factor>& fg); SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/** /**
* Return the set of variables involved in the factors (computes a set * Return the set of variables involved in the factors (computes a set
@ -73,8 +73,8 @@ public:
}; };
/* Template function implementation */ /* Template function implementation */
template<class FactorType> template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FactorType>& fg) { SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) { for (size_t i = 0; i < fg.size(); i++) {
if(fg[i]) { if(fg[i]) {
Factor::shared_ptr factor(new Factor(*fg[i])); Factor::shared_ptr factor(new Factor(*fg[i]));

10
inference/inference-inl.h
View File

@ -50,8 +50,8 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class FACTOR>
BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<Factor>& factorGraph) { BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) {
// Create a copy of the factor graph to eliminate in-place // Create a copy of the factor graph to eliminate in-place
FactorGraph<gtsam::Factor> eliminationGraph(factorGraph); FactorGraph<gtsam::Factor> eliminationGraph(factorGraph);
@ -261,7 +261,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
// Join the factors and eliminate the variable from the joint factor // Join the factors and eliminate the variable from the joint factor
tic("EliminateOne: Combine"); tic("EliminateOne: Combine");
typename FactorGraph::sharedFactor jointFactor(FactorGraph::factor_type::Combine(factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); typename FactorGraph::sharedFactor jointFactor(FactorGraph::Factor::Combine(factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions));
toc("EliminateOne: Combine"); toc("EliminateOne: Combine");
// Remove the original factors // Remove the original factors
@ -302,7 +302,7 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain
varIndex.permute(*permutation); varIndex.permute(*permutation);
FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size()); FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size());
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) { BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::factor_type(*factor)); typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::Factor(*factor));
permFactor->permuteWithInverse(*permutationInverse); permFactor->permuteWithInverse(*permutationInverse);
eliminationGraph.push_back(permFactor); eliminationGraph.push_back(permFactor);
} }
@ -316,7 +316,7 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain
FactorGraph marginal; marginal.reserve(variables.size()); FactorGraph marginal; marginal.reserve(variables.size());
typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
for(Index j=0; j<variables.size(); ++j, ++conditional) { for(Index j=0; j<variables.size(); ++j, ++conditional) {
typename FactorGraph::sharedFactor factor(new typename FactorGraph::factor_type(**conditional)); typename FactorGraph::sharedFactor factor(new typename FactorGraph::Factor(**conditional));
factor->permuteWithInverse(*permutation); factor->permuteWithInverse(*permutation);
marginal.push_back(factor); marginal.push_back(factor);
assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end()); assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end());

4
inference/inference.h
View File

@ -51,8 +51,8 @@ class Conditional;
* variables in order starting from 0. Special fast version for symbolic * variables in order starting from 0. Special fast version for symbolic
* elimination. * elimination.
*/ */
template<class Factor> template<class FACTOR>
static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<Factor>& factorGraph); static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph);
/** /**
* Eliminate a factor graph in its natural ordering, i.e. eliminating * Eliminate a factor graph in its natural ordering, i.e. eliminating

2
linear/GaussianISAM.h
View File

@ -45,7 +45,7 @@ public:
// update dimensions // update dimensions
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, newFactors) { BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, newFactors) {
for(typename FactorGraph::factor_type::const_iterator key = factor->begin(); key != factor->end(); ++key) { for(typename FactorGraph::Factor::const_iterator key = factor->begin(); key != factor->end(); ++key) {
if(*key >= dims_.size()) if(*key >= dims_.size())
dims_.resize(*key + 1); dims_.resize(*key + 1);
if(dims_[*key] == 0) if(dims_[*key] == 0)