113 lines
3.0 KiB
C++
113 lines
3.0 KiB
C++
/**
|
|
* @file FactorGraph.h
|
|
* @brief Factor Graph Base Class
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <list>
|
|
#include <map>
|
|
#include <vector>
|
|
#include <boost/shared_ptr.hpp>
|
|
#include "Factor.h"
|
|
#include "FGConfig.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
|
|
* In this class, however, only factor nodes are kept around.
|
|
*/
|
|
template<class T> class FactorGraph
|
|
{
|
|
public:
|
|
typedef typename boost::shared_ptr<T> shared_factor;
|
|
typedef typename std::vector<shared_factor>::iterator iterator;
|
|
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
|
|
|
|
protected:
|
|
/** Collection of factors */
|
|
std::vector<shared_factor> factors;
|
|
std::map<std::string, std::list<int> > node_to_factors_;
|
|
|
|
public:
|
|
|
|
/** get the factors to a specific node */
|
|
const std::list<int>& get_factors_to_node(const std::string& key) const {
|
|
return node_to_factors_[key];
|
|
}
|
|
|
|
/** STL like, return the iterator pointing to the first factor */
|
|
const_iterator begin() const {
|
|
return factors.begin();
|
|
}
|
|
|
|
/** STL like, return the iterator pointing to the last factor */
|
|
const_iterator end() const {
|
|
return factors.end();
|
|
}
|
|
|
|
/** clear the factor graph */
|
|
void clear(){
|
|
factors.clear();
|
|
node_to_factors_.clear();
|
|
}
|
|
|
|
/** Get a specific factor by index */
|
|
shared_factor operator[](size_t i) const {
|
|
return factors[i];
|
|
}
|
|
|
|
/** return the numbers of the factors in the factor graph */
|
|
inline size_t size() const { return factors.size(); }
|
|
|
|
/** Add a factor */
|
|
void push_back(shared_factor ptr_f) {factors.push_back(ptr_f);}
|
|
|
|
/** unnormalized error */
|
|
double error(const FGConfig& c) const {
|
|
double total_error = 0.;
|
|
/** iterate over all the factors to accumulate the log probabilities */
|
|
for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
|
|
total_error += (*factor)->error(c);
|
|
|
|
return total_error;
|
|
}
|
|
|
|
/** Unnormalized probability. O(n) */
|
|
double probPrime(const FGConfig& c) const {
|
|
return exp(-0.5*error(c));
|
|
}
|
|
|
|
/** print out graph */
|
|
void print(const std::string& s = "FactorGraph") const{
|
|
std::cout << s << std::endl;
|
|
printf("size: %d\n", (int)size());
|
|
for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
|
|
(*factor)->print();
|
|
}
|
|
|
|
/** Check equality */
|
|
bool equals(const FactorGraph& fg, double tol=1e-9) const
|
|
{
|
|
/** check whether the two factor graphs have the same number of factors */
|
|
if( factors.size() != fg.size() ) goto fail;
|
|
|
|
/** check whether the factors are the same */
|
|
for(size_t i=0;i<factors.size();i++)
|
|
if ( ! factors[i]->equals(*fg.factors[i], tol) ) goto fail; //TODO: Doesn't this force order of factor insertion?
|
|
return true;
|
|
|
|
fail:
|
|
print();
|
|
fg.print();
|
|
return false;
|
|
}
|
|
};
|
|
|
|
} // namespace gtsam
|