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

568
inference/FactorGraph-inl.h
View File

@ -26,7 +26,6 @@
#include "graph-inl.h"
#include "DSF.h"
#define INSTANTIATE_FACTOR_GRAPH(F) \
template class FactorGraph<F>; \
/*template boost::shared_ptr<F> removeAndCombineFactors(FactorGraph<F>&, const std::string&);*/ \
@ -36,21 +35,50 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class Factor>
template<class Conditional>
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet)
{
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::associateFactor(size_t index,
const sharedFactor& factor) {
// rtodo: Can optimize factor->keys to return a const reference
const list<Symbol> 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<class Factor>
template<class Conditional>
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
typename BayesNet<Conditional>::const_iterator it = bayesNet.begin();
for(; it != bayesNet.end(); it++) {
for (; it != bayesNet.end(); it++) {
sharedFactor factor(new Factor(*it));
push_back(factor);
}
}
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::print(const string& s) const {
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::push_back(sharedFactor factor) {
factors_.push_back(factor); // add the actual factor
if (factor == NULL) return;
size_t i = factors_.size() - 1; // index of factor
associateFactor(i, factor);
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
const_iterator factor = factors.begin();
for (; factor != factors.end(); factor++)
push_back(*factor);
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::print(const string& s) const {
cout << s << endl;
printf("size: %d\n", (int) size());
for (size_t i = 0; i < factors_.size(); i++) {
@ -58,12 +86,11 @@ void FactorGraph<Factor>::print(const string& s) const {
ss << "factor " << i << ":";
if (factors_[i] != NULL) factors_[i]->print(ss.str());
}
}
}
/* ************************************************************************* */
template<class Factor>
bool FactorGraph<Factor>::equals
(const FactorGraph<Factor>& fg, double tol) const {
/* ************************************************************************* */
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;
@ -76,113 +103,46 @@ bool FactorGraph<Factor>::equals
if (!f1->equals(*f2, tol)) return false;
}
return true;
}
}
/* ************************************************************************* */
template<class Factor>
size_t FactorGraph<Factor>::nrFactors() const {
/* ************************************************************************* */
template<class Factor>
size_t FactorGraph<Factor>::nrFactors() const {
size_t size_ = 0;
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
if (*factor != NULL) size_++;
return size_;
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::push_back(sharedFactor factor) {
factors_.push_back(factor); // add the actual factor
if (factor==NULL) return;
size_t i = factors_.size() - 1; // index of factor
associateFactor(i, factor);
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
const_iterator factor = factors.begin();
for (; factor!= factors.end(); factor++)
push_back(*factor);
}
/* ************************************************************************* */
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
BOOST_FOREACH(const Symbol& key, factors_[index]->keys()) {
indices_.at(key).remove(index);
}
}
// Replace the factor
factors_[index] = factor;
associateFactor(index, factor);
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::keys() const {
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::keys() const {
Ordering keys;
transform(indices_.begin(), indices_.end(),
back_inserter(keys), _Select1st<Indices::value_type>());
transform(indices_.begin(), indices_.end(), back_inserter(keys),
_Select1st<Indices::value_type> ());
return keys;
}
/* ************************************************************************* */
template<class Factor>
std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingletons() {
FactorGraph<Factor> singletonGraph;
set<Symbol> singletons;
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) {
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& singleton, new_singletons)
findAndRemoveFactors<vector<boost::shared_ptr<Factor> > >(singleton);
// exit when there are no more singletons
if (new_singletons.empty()) break;
}
return make_pair(singletonGraph, singletons);
}
/* ************************************************************************* */
/** O(1) */
/* ************************************************************************* */
template<class Factor>
list<size_t> FactorGraph<Factor>::factors(const Symbol& key) const {
return indices_.at(key);
}
/* ************************************************************************* */
/**
/* ************************************************************************* *
* 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 <class Key>
void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >& columns,
Ordering& ordering, const set<Symbol>& lastKeys) {
* ************************************************************************* */
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 */
@ -196,22 +156,23 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >&
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++) {
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
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;
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
cmember[j - 1] = 1; // force lastKeys to be at the end
}
j+=1;
j += 1;
}
if (!front_exists) { // if only 1 entries, set everything to 0...
for(int j = 0; j < n_col; j++)
for (int j = 0; j < n_col; j++)
cmember[j] = 0;
}
@ -222,19 +183,20 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >&
/* 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;
delete[] A; // delete symbolic A
delete[] cmember;
// Convert elimination ordering in p to an ordering
for(int j = 0; j < n_col; j++)
for (int j = 0; j < n_col; j++)
ordering.push_back(initialOrder[p[j]]);
delete [] p; // delete colamd result vector
}
delete[] p; // delete colamd result vector
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::getOrdering(Ordering& ordering, const set<Symbol>& lastKeys,
boost::optional<const set<Symbol>&> interested) const {
/* ************************************************************************* */
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.
@ -243,14 +205,14 @@ void FactorGraph<Factor>::getOrdering(Ordering& ordering, const set<Symbol>& las
int n_row = 0; /* colamd arg 1: number of rows in A */
// loop over all factors = rows
bool hasInterested = interested.is_initialized();
bool inserted;
bool hasInterested = scope.is_initialized();
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if (factor==NULL) continue;
if (factor == NULL) continue;
list<Symbol> keys = factor->keys();
inserted = false;
BOOST_FOREACH(const Symbol& key, keys) {
if (!hasInterested || interested->find(key) != interested->end()) {
if (!hasInterested || scope->find(key) != scope->end()) {
columns[key].push_back(n_row);
nrNonZeros++;
inserted = true;
@ -258,136 +220,52 @@ void FactorGraph<Factor>::getOrdering(Ordering& ordering, const set<Symbol>& las
}
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);
}
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{
/* ************************************************************************* */
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 {
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getOrdering(const set<Symbol>& scope) const {
Ordering ordering;
set<Symbol> lastKeys;
getOrdering(ordering, lastKeys);
getOrdering(ordering, lastKeys, scope);
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 {
/* ************************************************************************* */
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> template<class Key, class Factor2>
PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
return found;
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::associateFactor(size_t index, const sharedFactor& factor) {
const list<Symbol> keys = factor->keys(); // get keys for factor
// rtodo: Can optimize factor->keys to return a const reference
BOOST_FOREACH(const Symbol& key, keys) // for each key push i onto list
indices_[key].push_back(index);
}
/* ************************************************************************* */
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;
}
}
}
// 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);
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));
@ -397,51 +275,57 @@ PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
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++) {
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{
/* ************************************************************************* */
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"));
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);
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))
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 {
/* ************************************************************************* */
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++) {
for (size_t i = 0; i < size(); i++) {
const sharedFactor& f = factors_[i];
// retrieve the labels of all the keys
@ -454,29 +338,156 @@ std::pair<FactorGraph<Factor>, FactorGraph<Factor> > FactorGraph<Factor>::splitM
T.push_back(f);
set<Symbol>::const_iterator it = labels.begin();
Symbol root = *it;
for (it++; it!=labels.end(); it++)
for (it++; it != labels.end(); it++)
dsf = dsf.makeUnion(root, *it);
} else // otherwise add that factor to C
} else
// otherwise add that factor to C
C.push_back(f);
}
return make_pair(T,C);
}
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)
{
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;
}
/* ************************************************************************* */
template<class Factor> void FactorGraph<Factor>::checkGraphConsistency() const {
// Consistency check for debugging
/* ************************************************************************* */
template<class Factor>
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {
// 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>
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
BOOST_FOREACH(const Symbol& key, factors_[index]->keys())
{
indices_.at(key).remove(index);
}
}
// Replace the factor
factors_[index] = factor;
associateFactor(index, factor);
}
/* ************************************************************************* */
/** 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) {
// 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;
vector<sharedFactor> 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);
return found;
}
/* ************************************************************************* */
template<class Factor>
std::pair<FactorGraph<Factor> , set<Symbol> > FactorGraph<Factor>::removeSingletons() {
FactorGraph<Factor> singletonGraph;
set<Symbol> singletons;
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) {
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& singleton, new_singletons)
findAndRemoveFactors(singleton);
// exit when there are no more singletons
if (new_singletons.empty()) break;
}
return make_pair(singletonGraph, singletons);
}
/* ************************************************************************* */
template<class Factor>
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;
@ -484,6 +495,21 @@ FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Fa
fg.push_back(fg2);
return fg;
}
}
/* ************************************************************************* */
template<class Factor> boost::shared_ptr<Factor> removeAndCombineFactors(
FactorGraph<Factor>& factorGraph, const Symbol& key) {
// find and remove the factors associated with key
vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
// make a vector out of them and create a new factor
boost::shared_ptr<Factor> new_factor(new Factor(found));
// return it
return new_factor;
}
/* ************************************************************************* */
} // namespace gtsam

108
inference/FactorGraph.h
View File

@ -47,8 +47,20 @@ namespace gtsam {
typedef SymbolMap<std::list<size_t> > 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:
/** ------------------ Creating Factor Graphs ---------------------------- */
/** Default constructor */
FactorGraph() {}
@ -56,6 +68,14 @@ namespace gtsam {
template<class Conditional>
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 */
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<Factor>& factors);
/** replace a factor by index */
void replace(size_t index, sharedFactor factor);
/** return keys in some random order */
Ordering keys() const;
@ -101,18 +106,6 @@ namespace gtsam {
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
* @param key the key for the given node
@ -120,20 +113,22 @@ namespace gtsam {
std::list<size_t> 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
* Compute colamd ordering, including I/O, constrained ordering,
* and shared pointer version.
*/
template<class Factors>
Factors findAndRemoveFactors(const Symbol& key);
Ordering getOrdering() const;
boost::shared_ptr<Ordering> getOrdering_() const;
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
*/
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
*/
template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree,
@ -142,16 +137,37 @@ namespace gtsam {
/**
* 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
*/
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:
/** 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;
@ -163,15 +179,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<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key);
/**
* static function that combines two factor graphs
* @param const &fg1 Linear factor graph
* @param const &fg2 Linear factor graph
@ -180,5 +187,14 @@ namespace gtsam {
template<class Factor>
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

5
inference/JunctionTree-inl.h
View File

@ -51,11 +51,10 @@ namespace gtsam {
// collect the factors
typedef vector<typename FG::sharedFactor> Factors;
BOOST_FOREACH(const Symbol& frontal, clique->frontal_) {
Factors factors = fg.template findAndRemoveFactors<Factors>(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;
}

3
linear/GaussianFactorGraph.cpp
View File

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

27
tests/testGaussianFactorGraph.cpp
View File

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

2
tests/testSymbolicFactorGraph.cpp
View File

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