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

Re-organized FactorGraph, and de-templatized findAndRemoveFactors

release/4.3a0
Frank Dellaert 2010-07-14 13:55:32 +00:00
parent 5ce345adc6
commit 20b09e5383
6 changed files with 497 additions and 482 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

830
inference/FactorGraph-inl.h
View File

@ -26,7 +26,6 @@
#include "graph-inl.h" #include "graph-inl.h"
#include "DSF.h" #include "DSF.h"
#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 boost::shared_ptr<F> removeAndCombineFactors(FactorGraph<F>&, const std::string&);*/ \
@ -36,454 +35,481 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
template<class Conditional> void FactorGraph<Factor>::associateFactor(size_t index,
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) const sharedFactor& factor) {
{ // rtodo: Can optimize factor->keys to return a const reference
typename BayesNet<Conditional>::const_iterator it = bayesNet.begin(); const list<Symbol> keys = factor->keys(); // get keys for factor
for(; it != bayesNet.end(); it++) {
sharedFactor factor(new Factor(*it)); // for each key push i onto list
push_back(factor); BOOST_FOREACH(const Symbol& key, keys)
indices_[key].push_back(index);
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
void FactorGraph<Factor>::print(const string& s) const { template<class Conditional>
cout << s << endl; FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
printf("size: %d\n", (int) size()); typename BayesNet<Conditional>::const_iterator it = bayesNet.begin();
for (size_t i = 0; i < factors_.size(); i++) { for (; it != bayesNet.end(); it++) {
stringstream ss; sharedFactor factor(new Factor(*it));
ss << "factor " << i << ":"; push_back(factor);
if (factors_[i] != NULL) factors_[i]->print(ss.str()); }
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
bool FactorGraph<Factor>::equals void FactorGraph<Factor>::push_back(sharedFactor factor) {
(const FactorGraph<Factor>& fg, double tol) const { factors_.push_back(factor); // add the actual factor
/** check whether the two factor graphs have the same number of factors_ */ if (factor == NULL) return;
if (factors_.size() != fg.size()) return false;
/** check whether the factors_ are the same */ size_t i = factors_.size() - 1; // index of factor
for (size_t i = 0; i < factors_.size(); i++) { associateFactor(i, factor);
// 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<class Factor> template<class Factor>
size_t FactorGraph<Factor>::nrFactors() const { void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
size_t size_ = 0; const_iterator factor = factors.begin();
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) for (; factor != factors.end(); factor++)
if (*factor != NULL) size_++; push_back(*factor);
return size_; }
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
void FactorGraph<Factor>::push_back(sharedFactor factor) { void FactorGraph<Factor>::print(const string& s) const {
factors_.push_back(factor); // add the actual factor cout << s << endl;
if (factor==NULL) return; 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<class Factor>
} bool FactorGraph<Factor>::equals(const FactorGraph<Factor>& fg, double tol) const {
/** check whether the two factor graphs have the same number of factors_ */
if (factors_.size() != fg.size()) return false;
/* ************************************************************************* */ /** check whether the factors_ are the same */
template<class Factor> for (size_t i = 0; i < factors_.size(); i++) {
void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) { // TODO: Doesn't this force order of factor insertion?
const_iterator factor = factors.begin(); sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
for (; factor!= factors.end(); factor++) if (f1 == NULL && f2 == NULL) continue;
push_back(*factor); if (f1 == NULL || f2 == NULL) return false;
} if (!f1->equals(*f2, tol)) return false;
}
return true;
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
void FactorGraph<Factor>::replace(size_t index, sharedFactor factor) { size_t FactorGraph<Factor>::nrFactors() const {
if(index >= factors_.size()) size_t size_ = 0;
throw invalid_argument(boost::str(boost::format( for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
"Factor graph does not contain a factor with index %d.") % index)); if (*factor != NULL) size_++;
//if(factors_[index] == NULL) return size_;
// throw invalid_argument(boost::str(boost::format( }
// "Factor with index %d is NULL." % index)));
if(factors_[index] != NULL) { /* ************************************************************************* */
// Remove this factor from its variables' index lists template<class Factor>
BOOST_FOREACH(const Symbol& key, factors_[index]->keys()) { Ordering FactorGraph<Factor>::keys() const {
indices_.at(key).remove(index); Ordering keys;
} transform(indices_.begin(), indices_.end(), back_inserter(keys),
} _Select1st<Indices::value_type> ());
return keys;
}
// Replace the factor /* ************************************************************************* */
factors_[index] = factor; /** O(1) */
associateFactor(index, factor); /* ************************************************************************* */
} template<class Factor>
list<size_t> FactorGraph<Factor>::factors(const Symbol& key) const {
return indices_.at(key);
}
/* ************************************************************************* */ /* ************************************************************************* *
template<class Factor> * Call colamd given a column-major symbolic matrix A
Ordering FactorGraph<Factor>::keys() const { * @param n_col colamd arg 1: number of rows in A
Ordering keys; * @param n_row colamd arg 2: number of columns in A
transform(indices_.begin(), indices_.end(), * @param nrNonZeros number of non-zero entries in A
back_inserter(keys), _Select1st<Indices::value_type>()); * @param columns map from keys to a sparse column of non-zero row indices
return keys; * @param lastKeys set of keys that should appear last in the ordering
} * ************************************************************************* */
template<class Key>
void colamd(int n_col, int n_row, int nrNonZeros,
const map<Key, vector<int> >& columns, Ordering& ordering, const set<
Symbol>& lastKeys) {
/* ************************************************************************* */ // Convert to compressed column major format colamd wants it in (== MATLAB format!)
template<class Factor> int Alen = ccolamd_recommended(nrNonZeros, n_row, n_col); /* colamd arg 3: size of the array A */
std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingletons() { int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */
FactorGraph<Factor> singletonGraph; int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
set<Symbol> singletons; int * cmember = new int[n_col]; /* Constraint set of A, of size n_col */
while(true) { p[0] = 0;
// find all the singleton variables int j = 1;
Ordering new_singletons; int count = 0;
Symbol key; typedef typename map<Key, vector<int> >::const_iterator iterator;
list<size_t> indices; bool front_exists = false;
BOOST_FOREACH(boost::tie(key, indices), indices_) { vector<Key> initialOrder;
// find out the number of factors associated with the current key for (iterator it = columns.begin(); it != columns.end(); it++) {
size_t numValidFactors = 0; const Key& key = it->first;
BOOST_FOREACH(const size_t& i, indices) const vector<int>& column = it->second;
if (factors_[i]!=NULL) numValidFactors++; 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) { double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
new_singletons.push_back(key); int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
BOOST_FOREACH(const size_t& i, indices)
if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]); // 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<Symbol>& lastKeys,
boost::optional<const set<Symbol>&> 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<Symbol, 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;
list<Symbol> 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<class Factor>
Ordering FactorGraph<Factor>::getOrdering() const {
Ordering ordering;
set<Symbol> 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<Symbol> lastKeys;
getOrdering(*ordering, lastKeys);
return ordering;
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getOrdering(const set<Symbol>& scope) const {
Ordering ordering;
set<Symbol> lastKeys;
getOrdering(ordering, lastKeys, scope);
return ordering;
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getConstrainedOrdering(
const set<Symbol>& 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> void FactorGraph<Factor>::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<Symbol> 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) // Make sure each factor listed for a variable actually involves that variable
findAndRemoveFactors<vector<boost::shared_ptr<Factor> > >(singleton); BOOST_FOREACH(const SymbolMap<list<size_t> >::value_type& var, indices_)
{
// exit when there are no more singletons BOOST_FOREACH(size_t i, var.second)
if (new_singletons.empty()) break; {
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<Symbol> 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<class Factor>
void FactorGraph<Factor>::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)));
/* ************************************************************************* */ if (factors_[index] != NULL) {
/** // Remove this factor from its variables' index lists
* Call colamd given a column-major symbolic matrix A BOOST_FOREACH(const Symbol& key, factors_[index]->keys())
* @param n_col colamd arg 1: number of rows in A {
* @param n_row colamd arg 2: number of columns in A indices_.at(key).remove(index);
* @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 <class Key>
void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >& columns,
Ordering& ordering, const set<Symbol>& 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<Key, vector<int> >::const_iterator iterator;
bool front_exists = false;
vector<Key> initialOrder;
for(iterator it = columns.begin(); it != columns.end(); it++) {
const Key& 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;
} // Replace the factor
if (!front_exists) { // if only 1 entries, set everything to 0... factors_[index] = factor;
for(int j = 0; j < n_col; j++) associateFactor(index, factor);
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 */ /** find all non-NULL factors for a variable, then set factors to NULL */
/* ************************************************************************* */
template<class Factor>
vector<boost::shared_ptr<Factor> > FactorGraph<Factor>::findAndRemoveFactors(
const Symbol& key) {
// call colamd, result will be in p ************************************************* // find all factor indices associated with the key
/* TODO: returns (1) if successful, (0) otherwise*/ Indices::const_iterator it = indices_.find(key);
::ccolamd(n_row, n_col, Alen, A, p, knobs, stats, cmember); if (it == indices_.end()) throw std::invalid_argument(
// ********************************************************************************** "FactorGraph::findAndRemoveFactors: key " + (string) key + " not found");
delete [] A; // delete symbolic A const list<size_t>& factorsAssociatedWithKey = it->second;
delete [] cmember;
// Convert elimination ordering in p to an ordering vector<sharedFactor> found;
for(int j = 0; j < n_col; j++) BOOST_FOREACH(const size_t& i, factorsAssociatedWithKey) {
ordering.push_back(initialOrder[p[j]]); sharedFactor& fi = factors_.at(i); // throws exception !
delete [] p; // delete colamd result vector if (fi == NULL) continue; // skip NULL factors
} found.push_back(fi); // add to found
fi.reset(); // set factor to NULL == remove(i)
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::getOrdering(Ordering& ordering, const set<Symbol>& lastKeys,
boost::optional<const set<Symbol>&> 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<Symbol, 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 hasInterested = interested.is_initialized();
bool inserted;
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if (factor==NULL) continue;
list<Symbol> 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;
} }
}
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);
/* ************************************************************************* */ return found;
template<class Factor>
boost::shared_ptr<Ordering> FactorGraph<Factor>::getOrdering_() const{
boost::shared_ptr<Ordering> ordering(new Ordering);
set<Symbol> lastKeys;
getOrdering(*ordering, lastKeys);
return ordering;
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getOrdering() const {
Ordering ordering;
set<Symbol> lastKeys;
getOrdering(ordering, lastKeys);
return ordering;
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getOrdering(const set<Symbol>& interested) const {
Ordering ordering;
set<Symbol> lastKeys;
getOrdering(ordering, lastKeys, interested);
return ordering;
}
template<class Factor>
Ordering FactorGraph<Factor>::getConstrainedOrdering(const set<Symbol>& lastKeys) const {
Ordering ordering;
getOrdering(ordering, lastKeys);
return ordering;
}
/* ************************************************************************* */
/** O(1) */
/* ************************************************************************* */
template<class Factor>
list<size_t> FactorGraph<Factor>::factors(const Symbol& key) const {
return indices_.at(key);
}
/* ************************************************************************* */
/** find all non-NULL factors for a variable, then set factors to NULL */
/* ************************************************************************* */
template<class Factor> template<class Factors>
Factors FactorGraph<Factor>::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<size_t>& 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)
} }
indices_.erase(key); /* ************************************************************************* */
template<class Factor>
std::pair<FactorGraph<Factor> , set<Symbol> > FactorGraph<Factor>::removeSingletons() {
FactorGraph<Factor> singletonGraph;
set<Symbol> singletons;
return found; while (true) {
} // find all the singleton variables
Ordering new_singletons;
Symbol 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) {
template<class Factor> new_singletons.push_back(key);
void FactorGraph<Factor>::associateFactor(size_t index, const sharedFactor& factor) { BOOST_FOREACH(const size_t& i, indices)
const list<Symbol> keys = factor->keys(); // get keys for factor if (factors_[i] != NULL) singletonGraph.push_back(
// rtodo: Can optimize factor->keys to return a const reference factors_[i]);
}
}
singletons.insert(new_singletons.begin(), new_singletons.end());
BOOST_FOREACH(const Symbol& key, keys) // for each key push i onto list BOOST_FOREACH(const Symbol& singleton, new_singletons)
indices_[key].push_back(index); findAndRemoveFactors(singleton);
}
/* ************************************************************************* */ // exit when there are no more singletons
template<class Factor> void FactorGraph<Factor>::checkGraphConsistency() const { if (new_singletons.empty()) break;
// 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<Symbol> 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<list<size_t> >::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<Symbol> 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<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); return make_pair(singletonGraph, singletons);
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> template<class Factor>
std::pair<FactorGraph<Factor>, FactorGraph<Factor> > FactorGraph<Factor>::splitMinimumSpanningTree() const { FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1,
// create an empty factor graph T (tree) and factor graph C (constraints) const FactorGraph<Factor>& fg2) {
FactorGraph<Factor> T; // create new linear factor graph equal to the first one
FactorGraph<Factor> C; FactorGraph<Factor> fg = fg1;
DSF<Symbol> dsf(keys());
// while G is nonempty and T is not yet spanning // add the second factors_ in the graph
for (size_t i=0;i<size();i++) { fg.push_back(fg2);
const sharedFactor& f = factors_[i];
// retrieve the labels of all the keys return fg;
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);
}
/* ************************************************************************* */ /* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */ template<class Factor> boost::shared_ptr<Factor> removeAndCombineFactors(
/* ************************************************************************* */ FactorGraph<Factor>& factorGraph, const Symbol& key) {
template<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key)
{
typedef vector<boost::shared_ptr<Factor> > Factors;
Factors found = factorGraph.template findAndRemoveFactors<Factors>(key);
boost::shared_ptr<Factor> new_factor(new Factor(found));
return new_factor;
}
/* ************************************************************************* */ // find and remove the factors associated with key
template<class Factor> vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {
// create new linear factor graph equal to the first one
FactorGraph<Factor> fg = fg1;
// add the second factors_ in the graph // make a vector out of them and create a new factor
fg.push_back(fg2); boost::shared_ptr<Factor> new_factor(new Factor(found));
return fg; // return it
} return new_factor;
}
/* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

112
inference/FactorGraph.h
View File

@ -47,8 +47,20 @@ namespace gtsam {
typedef SymbolMap<std::list<size_t> > Indices; typedef SymbolMap<std::list<size_t> > Indices;
Indices 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<Symbol>& lastKeys,
boost::optional<const std::set<Symbol>&> scope = boost::none) const;
public: public:
/** ------------------ Creating Factor Graphs ---------------------------- */
/** Default constructor */ /** Default constructor */
FactorGraph() {} FactorGraph() {}
@ -56,6 +68,14 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
FactorGraph(const BayesNet<Conditional>& bayesNet); FactorGraph(const BayesNet<Conditional>& bayesNet);
/** Add a factor */
void push_back(sharedFactor factor);
/** push back many factors */
void push_back(const FactorGraph<Factor>& factors);
/** ------------------ Querying Factor Graphs ---------------------------- */
/** print out graph */ /** print out graph */
void print(const std::string& s = "FactorGraph") const; void print(const std::string& s = "FactorGraph") const;
@ -63,33 +83,18 @@ namespace gtsam {
bool equals(const FactorGraph& fg, double tol = 1e-9) const; bool equals(const FactorGraph& fg, double tol = 1e-9) const;
/** 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();}
inline const_iterator begin() const { 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(); } inline 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 {return factors_[i];} 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 */ /** return the number of factors and NULLS */
inline size_t size() const { return factors_.size();} inline 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;
/** Add a factor */
void push_back(sharedFactor factor);
/** push back many factors */
void push_back(const FactorGraph<Factor>& factors);
/** replace a factor by index */
void replace(size_t index, sharedFactor factor);
/** return keys in some random order */ /** return keys in some random order */
Ordering keys() const; Ordering keys() const;
@ -101,39 +106,29 @@ namespace gtsam {
return !(indices_.find(key)==indices_.end()); return !(indices_.find(key)==indices_.end());
} }
/** remove singleton variables and the related factors */
std::pair<FactorGraph<Factor>, std::set<Symbol> > removeSingletons();
/**
* Compute colamd ordering, including I/O, constrained ordering, and shared pointer version
*/
void getOrdering(Ordering& ordering, const std::set<Symbol>& lastKeys, boost::optional<const std::set<Symbol>&> interested = boost::none) const;
Ordering getOrdering() const;
Ordering getOrdering(const std::set<Symbol>& interested) const;
Ordering getConstrainedOrdering(const std::set<Symbol>& lastKeys) const;
boost::shared_ptr<Ordering> getOrdering_() const;
/** /**
* Return indices for all factors that involve the given node * Return indices for all factors that involve the given node
* @param key the key for the given node * @param key the key for the given node
*/ */
std::list<size_t> factors(const Symbol& key) const; std::list<size_t> factors(const Symbol& key) const;
/** /**
* find all the factors that involve the given node and remove them * Compute colamd ordering, including I/O, constrained ordering,
* from the factor graph * and shared pointer version.
* @param key the key for the given node */
*/ Ordering getOrdering() const;
template<class Factors> boost::shared_ptr<Ordering> getOrdering_() const;
Factors findAndRemoveFactors(const Symbol& key); Ordering getOrdering(const std::set<Symbol>& scope) const;
Ordering getConstrainedOrdering(const std::set<Symbol>& lastKeys) const;
/** /**
* find the minimum spanning tree using boost graph library * find the minimum spanning tree using boost graph library
*/ */
template<class Key, class Factor2> PredecessorMap<Key> findMinimumSpanningTree() const; template<class Key, class Factor2> PredecessorMap<Key>
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 * and the other corresponds to the rest of the factors
*/ */
template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree, template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree,
@ -142,16 +137,37 @@ namespace gtsam {
/** /**
* find the minimum spanning tree using DSF * find the minimum spanning tree using DSF
*/ */
std::pair<FactorGraph<Factor>, FactorGraph<Factor> > splitMinimumSpanningTree() const; std::pair<FactorGraph<Factor> , FactorGraph<Factor> >
splitMinimumSpanningTree() const;
/** /**
* Check consistency of the index map, useful for debugging * Check consistency of the index map, useful for debugging
*/ */
void checkGraphConsistency() const; 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<sharedFactor> findAndRemoveFactors(const Symbol& key);
/** remove singleton variables and the related factors */
std::pair<FactorGraph<Factor>, std::set<Symbol> > removeSingletons();
private: private:
/** Associate factor index with the variables connected to the factor */
void associateFactor(size_t index, const sharedFactor& factor);
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -162,15 +178,6 @@ namespace gtsam {
} }
}; // FactorGraph }; // 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<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key);
/** /**
* static function that combines two factor graphs * static function that combines two factor graphs
* @param const &fg1 Linear factor graph * @param const &fg1 Linear factor graph
@ -180,5 +187,14 @@ namespace gtsam {
template<class Factor> template<class Factor>
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2); FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& 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 Symbol& key);
} // namespace gtsam } // namespace gtsam

5
inference/JunctionTree-inl.h
View File

@ -51,10 +51,9 @@ namespace gtsam {
// collect the factors // collect the factors
typedef vector<typename FG::sharedFactor> Factors; typedef vector<typename FG::sharedFactor> Factors;
BOOST_FOREACH(const Symbol& frontal, clique->frontal_) { BOOST_FOREACH(const Symbol& frontal, clique->frontal_) {
Factors factors = fg.template findAndRemoveFactors<Factors>(frontal); Factors factors = fg.template findAndRemoveFactors(frontal);
BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) { BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors)
clique->push_back(factor_); clique->push_back(factor_);
}
} }
return clique; return clique;

3
linear/GaussianFactorGraph.cpp
View File

@ -201,8 +201,7 @@ std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
// find and remove all factors connected to key // find and remove all factors connected to key
typedef vector<GaussianFactor::shared_ptr> Factors; vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
Factors factors = findAndRemoveFactors<Factors>(key);
// Collect all dimensions as well as the set of separator keys // Collect all dimensions as well as the set of separator keys
set<Symbol> separator; set<Symbol> separator;

27
tests/testGaussianFactorGraph.cpp
View File

@ -552,32 +552,7 @@ TEST( GaussianFactorGraph, findAndRemoveFactors )
GaussianFactor::shared_ptr f2 = fg[2]; GaussianFactor::shared_ptr f2 = fg[2];
// call the function // call the function
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
<vector<GaussianFactor::shared_ptr> >("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<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors
<vector<GaussianFactor::shared_ptr> >("x1");
// Check the factors // Check the factors
CHECK(f0==factors[0]); CHECK(f0==factors[0]);

2
tests/testSymbolicFactorGraph.cpp
View File

@ -46,7 +46,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors )
SymbolicFactorGraph actual(factorGraph); SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0]; SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2]; SymbolicFactor::shared_ptr f3 = actual[2];
actual.findAndRemoveFactors<SymbolicFactorGraph>("x2"); actual.findAndRemoveFactors("x2");
// construct expected graph after find_factors_and_remove // construct expected graph after find_factors_and_remove
SymbolicFactorGraph expected; SymbolicFactorGraph expected;