diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index e9f7a4dc7..26d9bd4d0 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -42,7 +42,7 @@ EliminationTree::eliminate_() const { } // 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()); return EliminationResult(bayesNet, jointFactor); diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index f2478bd2e..953f9e9fb 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -25,7 +25,7 @@ class EliminationTree: public Testable > { public: - typedef boost::shared_ptr sharedFactor; + typedef boost::shared_ptr sharedFactor; typedef boost::shared_ptr > shared_ptr; typedef FACTORGRAPH FactorGraph; diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 7f0385a04..6cebefca9 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -40,29 +40,23 @@ #define INSTANTIATE_FACTOR_GRAPH(F) \ template class FactorGraph; \ - /*template boost::shared_ptr removeAndCombineFactors(FactorGraph&, const std::string&);*/ \ template FactorGraph combine(const FactorGraph&, const FactorGraph&); using namespace std; namespace gtsam { -// /* ************************************************************************* */ -// template -// FactorGraph::FactorGraph(const BayesNet& bayesNet, const Inference::Permutation& permutation) { -// } - /* ************************************************************************* */ - template - void FactorGraph::push_back(const FactorGraph& factors) { + template + void FactorGraph::push_back(const FactorGraph& factors) { const_iterator factor = factors.begin(); for (; factor != factors.end(); factor++) push_back(*factor); } /* ************************************************************************* */ - template - void FactorGraph::print(const string& s) const { + template + void FactorGraph::print(const string& s) const { cout << s << endl; printf("size: %d\n", (int) size()); for (size_t i = 0; i < factors_.size(); i++) { @@ -73,8 +67,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - bool FactorGraph::equals(const FactorGraph& fg, double tol) const { + template + bool FactorGraph::equals(const FactorGraph& fg, double tol) const { /** check whether the two factor graphs have the same number of factors_ */ if (factors_.size() != fg.size()) return false; @@ -90,232 +84,17 @@ namespace gtsam { } /* ************************************************************************* */ - template - size_t FactorGraph::nrFactors() const { + template + size_t FactorGraph::nrFactors() const { size_t size_ = 0; for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) if (*factor != NULL) 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 >& 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 */ -// 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 >::const_iterator iterator; -// bool front_exists = false; -// vector initialOrder; -// for (iterator it = columns.begin(); it != columns.end(); it++) { -// Index key = it->first; -// const vector& 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 -// void FactorGraph::getOrdering(Ordering& ordering, -// 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 -// 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& 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 -// Ordering FactorGraph::getOrdering() const { -// Ordering ordering; -// set lastKeys; -// getOrdering(ordering, lastKeys); -// return ordering; -// } -// -// /* ************************************************************************* */ -// template -// boost::shared_ptr FactorGraph::getOrdering_() const { -// boost::shared_ptr ordering(new Ordering); -// set lastKeys; -// getOrdering(*ordering, lastKeys); -// return ordering; -// } -// -// /* ************************************************************************* */ -// template -// Ordering FactorGraph::getOrdering(const set& scope) const { -// Ordering ordering; -// set lastKeys; -// getOrdering(ordering, lastKeys, scope); -// return ordering; -// } -// -// /* ************************************************************************* */ -// template -// Ordering FactorGraph::getConstrainedOrdering( -// const set& lastKeys) const { -// Ordering ordering; -// getOrdering(ordering, lastKeys); -// return ordering; -// } - -// /* ************************************************************************* */ -// template template -// PredecessorMap FactorGraph::findMinimumSpanningTree() const { -// -// SDGraph g = gtsam::toBoostGraph , Factor2, Key>( -// *this); -// -// // find minimum spanning tree -// vector::Vertex> p_map(boost::num_vertices(g)); -// prim_minimum_spanning_tree(g, &p_map[0]); -// -// // convert edge to string pairs -// PredecessorMap tree; -// typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; -// typename vector::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 template -// void FactorGraph::split(const PredecessorMap& tree, -// FactorGraph& Ab1, FactorGraph& 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 = 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 -// std::pair , FactorGraph > FactorGraph::splitMinimumSpanningTree() const { -// // create an empty factor graph T (tree) and factor graph C (constraints) -// FactorGraph T; -// FactorGraph C; -// DSF 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 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::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 - void FactorGraph::replace(size_t index, sharedFactor factor) { + template + void FactorGraph::replace(size_t index, sharedFactor factor) { if (index >= factors_.size()) throw invalid_argument(boost::str( boost::format("Factor graph does not contain a factor with index %d.") % index)); @@ -323,43 +102,6 @@ namespace gtsam { factors_[index] = factor; } -// /* ************************************************************************* */ -// template -// std::pair , set > FactorGraph::removeSingletons() { -// FactorGraph singletonGraph; -// set singletons; -// -// while (true) { -// // find all the singleton variables -// Ordering new_singletons; -// Index key; -// list 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 FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { @@ -372,19 +114,5 @@ namespace gtsam { return fg; } -// /* ************************************************************************* */ -// template boost::shared_ptr removeAndCombineFactors( -// FactorGraph& factorGraph, const Index& key) { -// -// // find and remove the factors associated with key -// vector > found = factorGraph.findAndRemoveFactors(key); -// -// // make a vector out of them and create a new factor -// boost::shared_ptr new_factor(new Factor(found)); -// -// // return it -// return new_factor; -// } - /* ************************************************************************* */ } // namespace gtsam diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index d898d0755..4a78ac64e 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -23,10 +23,7 @@ #include #include -//#include -//#include -//#include -//#include +#include #include #include @@ -42,12 +39,12 @@ namespace gtsam { * * Templated on the type of factors and values structure. */ - template - class FactorGraph: public Testable > { + template + class FactorGraph: public Testable > { public: - typedef Factor factor_type; - typedef boost::shared_ptr > shared_ptr; - typedef typename boost::shared_ptr sharedFactor; + typedef FACTOR Factor; + typedef boost::shared_ptr > shared_ptr; + typedef typename boost::shared_ptr sharedFactor; typedef typename std::vector::iterator iterator; typedef typename std::vector::const_iterator const_iterator; @@ -56,13 +53,6 @@ namespace gtsam { /** Collection of factors */ std::vector 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& lastKeys, -// boost::optional&> scope = boost::none) const; - public: /** ------------------ Creating Factor Graphs ---------------------------- */ @@ -83,7 +73,7 @@ namespace gtsam { void push_back(const boost::shared_ptr& factor); /** push back many factors */ - void push_back(const FactorGraph& factors); + void push_back(const FactorGraph& factors); /** push back many factors with an iterator */ template @@ -101,65 +91,29 @@ namespace gtsam { operator const std::vector&() const { return factors_; } /** STL begin and end, so we can use BOOST_FOREACH */ - inline const_iterator begin() const { return factors_.begin();} - inline const_iterator end() const { return factors_.end(); } + const_iterator begin() const { return factors_.begin();} + const_iterator end() const { return factors_.end(); } /** Get a specific factor by index */ - inline sharedFactor operator[](size_t i) const { assert(i getOrdering_() const; -// Ordering getOrdering(const std::set& scope) const; -// Ordering getConstrainedOrdering(const std::set& lastKeys) const; - -// /** -// * find the minimum spanning tree using boost graph library -// */ -// template -// PredecessorMap 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 -// void split(const PredecessorMap& tree, FactorGraph& Ab1, FactorGraph& Ab2) const; - -// /** -// * find the minimum spanning tree using DSF -// */ -// std::pair , FactorGraph > -// SL-NEEDED? splitMinimumSpanningTree() const; - -// /** -// * Check consistency of the index map, useful for debugging -// */ -// void checkGraphConsistency() const; - /** ----------------- Modifying Factor Graphs ---------------------------- */ /** STL begin and end, so we can use BOOST_FOREACH */ - inline iterator begin() { return factors_.begin();} - inline iterator end() { return factors_.end(); } + iterator begin() { return factors_.begin();} + iterator end() { return factors_.end(); } /** * Reserve space for the specified number of factors if you know in @@ -173,16 +127,6 @@ namespace gtsam { /** replace a factor by index */ 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 findAndRemoveFactors(Index key); -// -// /** remove singleton variables and the related factors */ -// std::pair, std::set > removeSingletons(); - private: /** Serialization function */ @@ -202,16 +146,6 @@ namespace gtsam { template 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 boost::shared_ptr -// removeAndCombineFactors(FactorGraph& factorGraph, const Index& key); - - /** * These functions are defined here because they are templated on an * additional parameter. Putting them in the -inl.h file would mean these @@ -220,43 +154,43 @@ namespace gtsam { */ /* ************************************************************************* */ - template + template template - FactorGraph::FactorGraph(const FactorGraph& factorGraph) { + FactorGraph::FactorGraph(const FactorGraph& factorGraph) { factors_.reserve(factorGraph.size()); BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) { if(factor) - this->push_back(sharedFactor(new Factor(*factor))); + this->push_back(sharedFactor(new FACTOR(*factor))); else this->push_back(sharedFactor()); } } /* ************************************************************************* */ - template + template template - FactorGraph::FactorGraph(const BayesNet& bayesNet) { + FactorGraph::FactorGraph(const BayesNet& bayesNet) { factors_.reserve(bayesNet.size()); BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) { - this->push_back(sharedFactor(new Factor(*cond))); + this->push_back(sharedFactor(new FACTOR(*cond))); } } /* ************************************************************************* */ - template + template template - inline void FactorGraph::push_back(const boost::shared_ptr& factor) { + inline void FactorGraph::push_back(const boost::shared_ptr& factor) { #ifndef NDEBUG - factors_.push_back(boost::dynamic_pointer_cast(factor)); // add the actual factor + factors_.push_back(boost::dynamic_pointer_cast(factor)); // add the actual factor #else - factors_.push_back(boost::static_pointer_cast(factor)); + factors_.push_back(boost::static_pointer_cast(factor)); #endif } /* ************************************************************************* */ - template + template template - void FactorGraph::push_back(Iterator firstFactor, Iterator lastFactor) { + void FactorGraph::push_back(Iterator firstFactor, Iterator lastFactor) { Iterator factor = firstFactor; while(factor != lastFactor) this->push_back(*(factor++)); diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index eaa40b7d1..6bc7646cd 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -72,7 +72,7 @@ namespace gtsam { Index maxVar = 0; for(size_t i=0; ibegin(), fg[i]->end()); + typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end()); if(min == fg[i]->end()) lowestOrdered[i] = numeric_limits::max(); else { @@ -168,7 +168,7 @@ namespace gtsam { // Now that we know which factors and variables, and where variables // come from and go to, create and eliminate the new joint factor. 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"); tic("JT 2.3 Eliminate"); typename FG::bayesnet_type::shared_ptr fragment = jointFactor->eliminate(current->frontal.size()); diff --git a/inference/JunctionTree.h b/inference/JunctionTree.h index b1273c9cd..6ae7cee1a 100644 --- a/inference/JunctionTree.h +++ b/inference/JunctionTree.h @@ -46,7 +46,7 @@ namespace gtsam { typedef typename ClusterTree::Cluster Clique; typedef typename Clique::shared_ptr sharedClique; - typedef class BayesTree BayesTree; + typedef class BayesTree BayesTree; // And we will frequently refer to a symbolic Bayes tree typedef gtsam::BayesTree SymbolicBayesTree; diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 395388d5a..4f3659673 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -57,8 +57,8 @@ public: /** * Construct from a factor graph of any type */ - template - SymbolicFactorGraph(const FactorGraph& fg); + template + SymbolicFactorGraph(const FactorGraph& fg); /** * Return the set of variables involved in the factors (computes a set @@ -73,8 +73,8 @@ public: }; /* Template function implementation */ -template -SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { +template +SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { for (size_t i = 0; i < fg.size(); i++) { if(fg[i]) { Factor::shared_ptr factor(new Factor(*fg[i])); diff --git a/inference/inference-inl.h b/inference/inference-inl.h index bace56a78..d72cec464 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -50,8 +50,8 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons } /* ************************************************************************* */ -template -BayesNet::shared_ptr Inference::EliminateSymbolic(const FactorGraph& factorGraph) { +template +BayesNet::shared_ptr Inference::EliminateSymbolic(const FactorGraph& factorGraph) { // Create a copy of the factor graph to eliminate in-place FactorGraph eliminationGraph(factorGraph); @@ -261,7 +261,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable // Join the factors and eliminate the variable from the joint factor 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"); // Remove the original factors @@ -302,7 +302,7 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain varIndex.permute(*permutation); FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size()); 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); eliminationGraph.push_back(permFactor); } @@ -316,7 +316,7 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain FactorGraph marginal; marginal.reserve(variables.size()); typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); for(Index j=0; jpermuteWithInverse(*permutation); marginal.push_back(factor); assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end()); diff --git a/inference/inference.h b/inference/inference.h index bf8c8d6a6..9f18b376f 100644 --- a/inference/inference.h +++ b/inference/inference.h @@ -51,8 +51,8 @@ class Conditional; * variables in order starting from 0. Special fast version for symbolic * elimination. */ - template - static BayesNet::shared_ptr EliminateSymbolic(const FactorGraph& factorGraph); + template + static BayesNet::shared_ptr EliminateSymbolic(const FactorGraph& factorGraph); /** * Eliminate a factor graph in its natural ordering, i.e. eliminating diff --git a/linear/GaussianISAM.h b/linear/GaussianISAM.h index ef8b3cde1..343964e2f 100644 --- a/linear/GaussianISAM.h +++ b/linear/GaussianISAM.h @@ -45,7 +45,7 @@ public: // update dimensions 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()) dims_.resize(*key + 1); if(dims_[*key] == 0)