12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/inference/FactorGraph-inl.h

514 lines
18 KiB
C++
Raw Blame History

/**
* @file FactorGraph-inl.h
* This is a template definition file, include it where needed (only!)
* so that the appropriate code is generated and link errors avoided.
* @brief Factor Graph Base Class
* @author Carlos Nieto
* @author Frank Dellaert
* @author Alireza Fathi
* @author Michael Kaess
*/
#pragma once
#include <list>
#include <sstream>
#include <stdexcept>
#include <functional>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <gtsam/colamd/ccolamd.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/base/DSF.h>
#define INSTANTIATE_FACTOR_GRAPH(F) \
template class FactorGraph<F>; \
/*template boost::shared_ptr<F> removeAndCombineFactors(FactorGraph<F>&, const std::string&);*/ \
template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&);
using namespace std;
namespace gtsam {
/* ************************************************************************* */
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++) {
sharedFactor factor(new Factor(*it));
push_back(factor);
}
}
/* ************************************************************************* */
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++) {
stringstream ss;
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 {
/** 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 */
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<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>
Ordering FactorGraph<Factor>::keys() const {
Ordering keys;
transform(indices_.begin(), indices_.end(), back_inserter(keys),
_Select1st<Indices::value_type> ());
return keys;
}
/* ************************************************************************* */
/** 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) {
// 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;
}
if (!front_exists) { // if only 1 entries, set everything to 0...
for (int j = 0; j < n_col; j++)
cmember[j] = 0;
}
double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
// call colamd, result will be in p *************************************************
/* TODO: returns (1) if successful, (0) otherwise*/
::ccolamd(n_row, n_col, Alen, A, p, knobs, stats, cmember);
// **********************************************************************************
delete[] A; // delete symbolic A
delete[] cmember;
// Convert elimination ordering in p to an ordering
for (int j = 0; j < n_col; j++)
ordering.push_back(initialOrder[p[j]]);
delete[] p; // delete colamd result vector
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::getOrdering(Ordering& ordering,
const set<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;
}
}
}
// 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);
vector<sharedFactor> found;
if (it != indices_.end()) {
const list<size_t>& factorsAssociatedWithKey = it->second;
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;
// add the second factors_ in the graph
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
Reference in New Issue View Git Blame Copy Permalink