diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 21ca81f64..725d77413 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -26,7 +26,6 @@ #include "graph-inl.h" #include "DSF.h" - #define INSTANTIATE_FACTOR_GRAPH(F) \ template class FactorGraph; \ /*template boost::shared_ptr removeAndCombineFactors(FactorGraph&, const std::string&);*/ \ @@ -36,454 +35,481 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -template -template -FactorGraph::FactorGraph(const BayesNet& bayesNet) -{ - typename BayesNet::const_iterator it = bayesNet.begin(); - for(; it != bayesNet.end(); it++) { - sharedFactor factor(new Factor(*it)); - push_back(factor); + /* ************************************************************************* */ + template + void FactorGraph::associateFactor(size_t index, + const sharedFactor& factor) { + // rtodo: Can optimize factor->keys to return a const reference + const list keys = factor->keys(); // get keys for factor + + // for each key push i onto list + BOOST_FOREACH(const Symbol& key, keys) + indices_[key].push_back(index); } -} -/* ************************************************************************* */ -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++) { - stringstream ss; - ss << "factor " << i << ":"; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); + /* ************************************************************************* */ + template + template + FactorGraph::FactorGraph(const BayesNet& bayesNet) { + typename BayesNet::const_iterator it = bayesNet.begin(); + for (; it != bayesNet.end(); it++) { + sharedFactor factor(new Factor(*it)); + push_back(factor); + } } -} -/* ************************************************************************* */ -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; + /* ************************************************************************* */ + template + void FactorGraph::push_back(sharedFactor factor) { + factors_.push_back(factor); // add the actual factor + if (factor == NULL) return; - /** check whether the factors_ are the same */ - for (size_t i = 0; i < factors_.size(); i++) { - // TODO: Doesn't this force order of factor insertion? - sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; - if (!f1->equals(*f2, tol)) return false; + size_t i = factors_.size() - 1; // index of factor + associateFactor(i, factor); } - return true; -} -/* ************************************************************************* */ -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_; -} + /* ************************************************************************* */ + template + void FactorGraph::push_back(const FactorGraph& factors) { + const_iterator factor = factors.begin(); + for (; factor != factors.end(); factor++) + push_back(*factor); + } -/* ************************************************************************* */ -template -void FactorGraph::push_back(sharedFactor factor) { - factors_.push_back(factor); // add the actual factor - if (factor==NULL) return; + /* ************************************************************************* */ + 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++) { + stringstream ss; + ss << "factor " << i << ":"; + if (factors_[i] != NULL) factors_[i]->print(ss.str()); + } + } - size_t i = factors_.size() - 1; // index of factor - associateFactor(i, factor); -} + /* ************************************************************************* */ + 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; -/* ************************************************************************* */ -template -void FactorGraph::push_back(const FactorGraph& factors) { - const_iterator factor = factors.begin(); - for (; factor!= factors.end(); factor++) - push_back(*factor); -} + /** check whether the factors_ are the same */ + for (size_t i = 0; i < factors_.size(); i++) { + // TODO: Doesn't this force order of factor insertion? + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; + if (f1 == NULL && f2 == NULL) continue; + if (f1 == NULL || f2 == NULL) return false; + if (!f1->equals(*f2, tol)) return false; + } + return true; + } -/* ************************************************************************* */ -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)); - //if(factors_[index] == NULL) - // throw invalid_argument(boost::str(boost::format( - // "Factor with index %d is NULL." % index))); + /* ************************************************************************* */ + 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_; + } - if(factors_[index] != NULL) { - // Remove this factor from its variables' index lists - BOOST_FOREACH(const Symbol& key, factors_[index]->keys()) { - indices_.at(key).remove(index); - } - } + /* ************************************************************************* */ + template + Ordering FactorGraph::keys() const { + Ordering keys; + transform(indices_.begin(), indices_.end(), back_inserter(keys), + _Select1st ()); + return keys; + } - // Replace the factor - factors_[index] = factor; - associateFactor(index, factor); -} + /* ************************************************************************* */ + /** O(1) */ + /* ************************************************************************* */ + template + list FactorGraph::factors(const Symbol& key) const { + return indices_.at(key); + } -/* ************************************************************************* */ -template -Ordering FactorGraph::keys() const { - Ordering keys; - transform(indices_.begin(), indices_.end(), - back_inserter(keys), _Select1st()); - return keys; -} + /* ************************************************************************* * + * 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 + * ************************************************************************* */ + template + void colamd(int n_col, int n_row, int nrNonZeros, + const map >& columns, Ordering& ordering, const set< + Symbol>& lastKeys) { -/* ************************************************************************* */ -template -std::pair, set > FactorGraph::removeSingletons() { - FactorGraph singletonGraph; - set singletons; + // 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 */ - while(true) { - // find all the singleton variables - Ordering new_singletons; - Symbol 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++; + p[0] = 0; + int j = 1; + int count = 0; + typedef typename map >::const_iterator iterator; + bool front_exists = false; + vector initialOrder; + for (iterator it = columns.begin(); it != columns.end(); it++) { + const Key& 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; + } - if (numValidFactors == 1) { - new_singletons.push_back(key); - BOOST_FOREACH(const size_t& i, indices) - if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]); + 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; + list keys = factor->keys(); + inserted = false; + BOOST_FOREACH(const Symbol& 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< + Factor>& 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::checkGraphConsistency() const { + // Consistency check for debugging + + // Make sure each factor is listed in its variables index lists + for (size_t i = 0; i < factors_.size(); i++) { + if (factors_[i] == NULL) { + cout << "** Warning: factor " << i << " is NULL" << endl; + } else { + // Get involved variables + list keys = factors_[i]->keys(); + + // Make sure each involved variable is listed as being associated with this factor + BOOST_FOREACH(const Symbol& var, keys) + { + if (std::find(indices_.at(var).begin(), indices_.at(var).end(), + i) == indices_.at(var).end()) cout + << "*** Factor graph inconsistency: " << (string) var + << " is not mapped to factor " << i << endl; + } } } - singletons.insert(new_singletons.begin(), new_singletons.end()); - BOOST_FOREACH(const Symbol& singleton, new_singletons) - findAndRemoveFactors > >(singleton); - - // exit when there are no more singletons - if (new_singletons.empty()) break; + // Make sure each factor listed for a variable actually involves that variable + BOOST_FOREACH(const SymbolMap >::value_type& var, indices_) + { + BOOST_FOREACH(size_t i, var.second) + { + if (i >= factors_.size()) { + cout << "*** Factor graph inconsistency: " + << (string) var.first << " lists factor " << i + << " but the graph does not contain this many factors." + << endl; + } + if (factors_[i] == NULL) { + cout << "*** Factor graph inconsistency: " + << (string) var.first << " lists factor " << i + << " but this factor is set to NULL." << endl; + } + list keys = factors_[i]->keys(); + if (std::find(keys.begin(), keys.end(), var.first) + == keys.end()) { + cout << "*** Factor graph inconsistency: " + << (string) var.first << " lists factor " << i + << " but this factor does not involve this variable." + << endl; + } + } + } } - return make_pair(singletonGraph, singletons); -} + /* ************************************************************************* */ + 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)); + //if(factors_[index] == NULL) + // throw invalid_argument(boost::str(boost::format( + // "Factor with index %d is NULL." % index))); -/* ************************************************************************* */ -/** - * 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 - */ -template -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 typename map >::const_iterator iterator; - bool front_exists = false; - vector initialOrder; - for(iterator it = columns.begin(); it != columns.end(); it++) { - const Key& 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 + if (factors_[index] != NULL) { + // Remove this factor from its variables' index lists + BOOST_FOREACH(const Symbol& key, factors_[index]->keys()) + { + indices_.at(key).remove(index); + } } - j+=1; - } - if (!front_exists) { // if only 1 entries, set everything to 0... - for(int j = 0; j < n_col; j++) - cmember[j] = 0; + + // Replace the factor + factors_[index] = factor; + associateFactor(index, factor); } - double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + /* ************************************************************************* */ + /** find all non-NULL factors for a variable, then set factors to NULL */ + /* ************************************************************************* */ + template + vector > FactorGraph::findAndRemoveFactors( + const Symbol& key) { - // 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; + // find all factor indices associated with the key + Indices::const_iterator it = indices_.find(key); + if (it == indices_.end()) throw std::invalid_argument( + "FactorGraph::findAndRemoveFactors: key " + (string) key + " not found"); + const list& factorsAssociatedWithKey = it->second; - // 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&> interested) 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 hasInterested = interested.is_initialized(); - bool inserted; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if (factor==NULL) continue; - list keys = factor->keys(); - inserted = false; - BOOST_FOREACH(const Symbol& key, keys) { - if (!hasInterested || interested->find(key) != interested->end()) { - columns[key].push_back(n_row); - nrNonZeros++; - inserted = true; + vector found; + BOOST_FOREACH(const size_t& i, factorsAssociatedWithKey) { + sharedFactor& fi = factors_.at(i); // throws exception ! + if (fi == NULL) continue; // skip NULL factors + found.push_back(fi); // add to found + fi.reset(); // set factor to NULL == remove(i) } - } - 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); -} + indices_.erase(key); -/* ************************************************************************* */ -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 { - Ordering ordering; - set lastKeys; - getOrdering(ordering, lastKeys); - return ordering; -} - -/* ************************************************************************* */ -template -Ordering FactorGraph::getOrdering(const set& interested) const { - Ordering ordering; - set lastKeys; - getOrdering(ordering, lastKeys, interested); - return ordering; -} - -template -Ordering FactorGraph::getConstrainedOrdering(const set& lastKeys) const { - Ordering ordering; - getOrdering(ordering, lastKeys); - return ordering; -} - -/* ************************************************************************* */ -/** O(1) */ -/* ************************************************************************* */ -template -list FactorGraph::factors(const Symbol& key) const { - return indices_.at(key); -} - -/* ************************************************************************* */ -/** find all non-NULL factors for a variable, then set factors to NULL */ -/* ************************************************************************* */ -template template -Factors FactorGraph::findAndRemoveFactors(const Symbol& key) { - - // find all factor indices associated with the key - Indices::const_iterator it = indices_.find(key); - if (it==indices_.end()) - throw std::invalid_argument( - "FactorGraph::findAndRemoveFactors: key " - + (string)key + " not found"); - const list& factorsAssociatedWithKey = it->second; - - Factors found; - BOOST_FOREACH(const size_t& i, factorsAssociatedWithKey) { - sharedFactor& fi = factors_.at(i); // throws exception ! - if(fi == NULL) continue; // skip NULL factors - found.push_back(fi); // add to found - fi.reset(); // set factor to NULL == remove(i) + return found; } - indices_.erase(key); + /* ************************************************************************* */ + template + std::pair , set > FactorGraph::removeSingletons() { + FactorGraph singletonGraph; + set singletons; - return found; -} + while (true) { + // find all the singleton variables + Ordering new_singletons; + Symbol 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++; -/* ************************************************************************* */ -template -void FactorGraph::associateFactor(size_t index, const sharedFactor& factor) { - const list keys = factor->keys(); // get keys for factor - // rtodo: Can optimize factor->keys to return a const reference + 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 Symbol& key, keys) // for each key push i onto list - indices_[key].push_back(index); -} + BOOST_FOREACH(const Symbol& singleton, new_singletons) + findAndRemoveFactors(singleton); -/* ************************************************************************* */ -template void FactorGraph::checkGraphConsistency() const { - // Consistency check for debugging - - // Make sure each factor is listed in its variables index lists - for(size_t i=0; i keys = factors_[i]->keys(); - - // Make sure each involved variable is listed as being associated with this factor - BOOST_FOREACH(const Symbol& var, keys) { - if(std::find(indices_.at(var).begin(), indices_.at(var).end(), i) == indices_.at(var).end()) - cout << "*** Factor graph inconsistency: " << (string)var << " is not mapped to factor " << i << endl; - } - } - } - - // Make sure each factor listed for a variable actually involves that variable - BOOST_FOREACH(const SymbolMap >::value_type& var, indices_) { - BOOST_FOREACH(size_t i, var.second) { - if(i >= factors_.size()) { - cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " << - i << " but the graph does not contain this many factors." << endl; - } - if(factors_[i] == NULL) { - cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " << - i << " but this factor is set to NULL." << endl; - } - list keys = factors_[i]->keys(); - if(std::find(keys.begin(), keys.end(), var.first) == keys.end()) { - cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " << - i << " but this factor does not involve this variable." << endl; - } - } - } -} - -/* ************************************************************************* */ -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; + // exit when there are no more singletons + if (new_singletons.empty()) break; } - boost::shared_ptr factor2 = boost::dynamic_pointer_cast(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); + return make_pair(singletonGraph, singletons); } -} -/* ************************************************************************* */ -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()); + /* ************************************************************************* */ + template + FactorGraph combine(const FactorGraph& fg1, + const FactorGraph& fg2) { + // create new linear factor graph equal to the first one + FactorGraph fg = fg1; - // while G is nonempty and T is not yet spanning - for (size_t i=0;i 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 fg; } - return make_pair(T,C); -} -/* ************************************************************************* */ -/* find factors and remove them from the factor graph: O(n) */ -/* ************************************************************************* */ -template boost::shared_ptr -removeAndCombineFactors(FactorGraph& factorGraph, const Symbol& key) -{ - typedef vector > Factors; - Factors found = factorGraph.template findAndRemoveFactors(key); - boost::shared_ptr new_factor(new Factor(found)); - return new_factor; -} + /* ************************************************************************* */ + template boost::shared_ptr removeAndCombineFactors( + FactorGraph& factorGraph, const Symbol& key) { -/* ************************************************************************* */ -template -FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { - // create new linear factor graph equal to the first one - FactorGraph fg = fg1; + // find and remove the factors associated with key + vector > found = factorGraph.findAndRemoveFactors(key); - // add the second factors_ in the graph - fg.push_back(fg2); + // make a vector out of them and create a new factor + boost::shared_ptr new_factor(new Factor(found)); - return fg; -} + // return it + return new_factor; + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 576d157aa..b221ba09b 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -47,8 +47,20 @@ namespace gtsam { typedef SymbolMap > Indices; Indices indices_; + /** Associate factor index with the variables connected to the factor */ + void associateFactor(size_t index, const sharedFactor& factor); + + /** + * 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 ---------------------------- */ + /** Default constructor */ FactorGraph() {} @@ -56,6 +68,14 @@ namespace gtsam { template FactorGraph(const BayesNet& bayesNet); + /** Add a factor */ + void push_back(sharedFactor factor); + + /** push back many factors */ + void push_back(const FactorGraph& factors); + + /** ------------------ Querying Factor Graphs ---------------------------- */ + /** print out graph */ void print(const std::string& s = "FactorGraph") const; @@ -63,33 +83,18 @@ namespace gtsam { bool equals(const FactorGraph& fg, double tol = 1e-9) const; /** STL begin and end, so we can use BOOST_FOREACH */ - - inline iterator begin() { return factors_.begin();} inline const_iterator begin() const { return factors_.begin();} - inline iterator end() { return factors_.end(); } inline const_iterator end() const { return factors_.end(); } /** Get a specific factor by index */ inline sharedFactor operator[](size_t i) const {return factors_[i];} - /** delete factor without re-arranging indexes by inserting a NULL pointer */ - inline void remove(size_t i) { factors_[i].reset();} - /** return the number of factors and NULLS */ inline size_t size() const { return factors_.size();} /** return the number valid factors */ size_t nrFactors() const; - /** Add a factor */ - void push_back(sharedFactor factor); - - /** push back many factors */ - void push_back(const FactorGraph& factors); - - /** replace a factor by index */ - void replace(size_t index, sharedFactor factor); - /** return keys in some random order */ Ordering keys() const; @@ -101,39 +106,29 @@ namespace gtsam { return !(indices_.find(key)==indices_.end()); } - /** remove singleton variables and the related factors */ - std::pair, std::set > removeSingletons(); - - /** - * Compute colamd ordering, including I/O, constrained ordering, and shared pointer version - */ - void getOrdering(Ordering& ordering, const std::set& lastKeys, boost::optional&> interested = boost::none) const; - Ordering getOrdering() const; - Ordering getOrdering(const std::set& interested) const; - Ordering getConstrainedOrdering(const std::set& lastKeys) const; - boost::shared_ptr getOrdering_() const; - /** * Return indices for all factors that involve the given node * @param key the key for the given node */ std::list factors(const Symbol& key) const; - /** - * find all the factors that involve the given node and remove them - * from the factor graph - * @param key the key for the given node - */ - template - Factors findAndRemoveFactors(const Symbol& key); + /** + * Compute colamd ordering, including I/O, constrained ordering, + * and shared pointer version. + */ + Ordering getOrdering() const; + boost::shared_ptr 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; + template PredecessorMap + findMinimumSpanningTree() const; /** - * Split the graph into two parts: one corresponds to the given spanning tre, + * 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, @@ -142,16 +137,37 @@ namespace gtsam { /** * find the minimum spanning tree using DSF */ - std::pair, FactorGraph > splitMinimumSpanningTree() const; + std::pair , FactorGraph > + 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(); } + + /** delete factor without re-arranging indexes by inserting a NULL pointer */ + inline void remove(size_t i) { factors_[i].reset();} + + /** 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(const Symbol& key); + + /** remove singleton variables and the related factors */ + std::pair, std::set > removeSingletons(); + private: - /** Associate factor index with the variables connected to the factor */ - void associateFactor(size_t index, const sharedFactor& factor); /** Serialization function */ friend class boost::serialization::access; @@ -162,15 +178,6 @@ namespace gtsam { } }; // FactorGraph - /** - * 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 Symbol& key); - /** * static function that combines two factor graphs * @param const &fg1 Linear factor graph @@ -180,5 +187,14 @@ 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 Symbol& key); + } // namespace gtsam diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index 7e23398e2..21b8fbaf6 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -51,10 +51,9 @@ namespace gtsam { // collect the factors typedef vector Factors; BOOST_FOREACH(const Symbol& frontal, clique->frontal_) { - Factors factors = fg.template findAndRemoveFactors(frontal); - BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) { + Factors factors = fg.template findAndRemoveFactors(frontal); + BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) clique->push_back(factor_); - } } return clique; diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index 560daf1a4..b7d00edda 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -201,8 +201,7 @@ std::pair combineFactorsAndCreateMatrix( GaussianConditional::shared_ptr GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { // find and remove all factors connected to key - typedef vector Factors; - Factors factors = findAndRemoveFactors(key); + vector factors = findAndRemoveFactors(key); // Collect all dimensions as well as the set of separator keys set separator; diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index a6f1d569f..adad96df7 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -552,32 +552,7 @@ TEST( GaussianFactorGraph, findAndRemoveFactors ) GaussianFactor::shared_ptr f2 = fg[2]; // call the function - vector factors = fg.findAndRemoveFactors - >("x1"); - - // Check the factors - CHECK(f0==factors[0]); - CHECK(f1==factors[1]); - CHECK(f2==factors[2]); - - // CHECK if the factors are deleted from the factor graph - LONGS_EQUAL(1,fg.nrFactors()); - } - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, findAndRemoveFactors_twice ) -{ - // create the graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - - // We expect to remove these three factors: 0, 1, 2 - GaussianFactor::shared_ptr f0 = fg[0]; - GaussianFactor::shared_ptr f1 = fg[1]; - GaussianFactor::shared_ptr f2 = fg[2]; - - // call the function - vector factors = fg.findAndRemoveFactors - >("x1"); + vector factors = fg.findAndRemoveFactors("x1"); // Check the factors CHECK(f0==factors[0]); diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index 123d7573e..1222c7261 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -46,7 +46,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors ) SymbolicFactorGraph actual(factorGraph); SymbolicFactor::shared_ptr f1 = actual[0]; SymbolicFactor::shared_ptr f3 = actual[2]; - actual.findAndRemoveFactors("x2"); + actual.findAndRemoveFactors("x2"); // construct expected graph after find_factors_and_remove SymbolicFactorGraph expected;