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

Moved old elimination tree to gtsam_experimental

release/4.3a0
Richard Roberts 2010-10-13 22:07:52 +00:00
parent 3c8b10505c
commit 47b11749fe
5 changed files with 2 additions and 444 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
inference/EliminationTree-inl.h
View File

@ -1,210 +0,0 @@
/*
* EliminationTree-inl.h
* Created on: Feb 4, 2010
* @Author: Kai Ni
* @Author: Frank Dellaert
* @brief: The elimination tree, template bodies
*/
#pragma once
#include <stdexcept>
#include <functional>
#include <boost/foreach.hpp>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
// template<class FG>
// void EliminationTree<FG>::add(const FG& fg, Index j) {
// sharedNode node(new Node(fg, j));
// add(node);
// }
/* ************************************************************************* */
template<class FG>
void EliminationTree<FG>::add(const sharedNode& node) {
assert(node->frontal.size() == 1);
Index j = node->frontal.front();
// Make a node and put it in the nodes_ array:
nodes_[j] = node;
// if the separator is empty, this is the root
if (node->separator.empty()) {
this->root_ = node;
}
else {
// find parent by iterating over all separator keys, and taking the lowest
// one in the ordering. That is the index of the parent clique.
vector<Index>::const_iterator parentIndex = min_element(node->separator.begin(), node->separator.end());
assert(parentIndex != node->separator.end());
// attach to parent
sharedNode& parent = nodes_[*parentIndex];
if (!parent) throw
invalid_argument("EliminationTree::add: parent clique does not exist");
node->parent() = parent;
parent->addChild(node);
}
}
/* ************************************************************************* */
// template<class FG>
// EliminationTree<FG>::EliminationTree(const OrderedGraphs& graphs) :
// nrVariables_(graphs.size()), nodes_(nrVariables_) {
//
// // Get ordering by (map first graphs)
// Ordering ordering;
// transform(graphs.begin(), graphs.end(), back_inserter(ordering),
// _Select1st<typename OrderedGraphs::value_type> ());
//
// // Create a temporary map from key to ordering index
// IndexTable<Symbol> indexTable(ordering);
//
// // Go over the collection in reverse elimination order
// // and add one node for every of the n variables.
// BOOST_REVERSE_FOREACH(const NamedGraph& namedGraph, graphs)
// add(namedGraph.second, namedGraph.first, indexTable);
// }
/* ************************************************************************* */
template<class FG>
EliminationTree<FG>::EliminationTree(FG& fg) {
static const bool debug = false;
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(fg.size() > 0) {
vector<deque<size_t> > clusters;
// Build clusters
{
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, fg) {
if(factor) {
typename FG::factor_type::const_iterator maxj = std::max_element(factor->begin(), factor->end());
if(maxj != factor->end() && *maxj > maxVar) maxVar = *maxj;
}
}
// Build index mapping from variable id to factor index - we only use
// the first variable because after this variable is eliminated the
// factor will no longer exist.
clusters.resize(maxVar+1);
for(size_t fi=0; fi<fg.size(); ++fi)
if(fg[fi] && !fg[fi]->keys().empty()) {
typename FG::factor_type::const_iterator firstvar = std::min_element(fg[fi]->begin(), fg[fi]->end());
assert(firstvar != fg[fi]->end());
clusters[*firstvar].push_back(fi);
}
}
// Create column index that will be modified during elimination - this is
// not the most efficient way of doing this, a modified version of
// Gilbert01bit would be more efficient.
vector<deque<size_t> > columnIndex = clusters;
nrVariables_ = columnIndex.size();
nodes_.resize(nrVariables_);
// Loop over all variables and get factors that are connected
OrderedGraphs graphs;
Nodes nodesToAdd; nodesToAdd.reserve(columnIndex.size());
for(Index j=0; j<columnIndex.size(); ++j) {
if(debug) cout << "Eliminating " << j << endl;
// The factor index of the new joint factor
size_t jointFactorI = fg.size();
// Get all of the factors associated with the variable.
// If the factor has not already been removed - I think this is
// somehow equivalent to the "find root" computation in Gilbert01bit.
vector<size_t> involvedFactors; involvedFactors.reserve(columnIndex[j].size());
BOOST_FOREACH(const size_t factorI, columnIndex[j]) {
if(fg[factorI]) involvedFactors.push_back(factorI);
}
if(!involvedFactors.empty()) {
// Compute a mapping (called variableSlots) *from* each involved
// variable that will be in the new joint factor *to* the slot in each
// removed factor in which that variable appears. For each variable,
// this is stored as a vector of slot numbers, stored in order of the
// removed factors. The slot number is the max integer value if the
// factor does not involve that variable.
typedef map<Index, vector<Index> > VariableSlots;
map<Index, vector<Index> > variableSlots;
FG removedFactors; removedFactors.reserve(involvedFactors.size());
size_t jointFactorPos = 0;
BOOST_FOREACH(const size_t factorI, involvedFactors) {
// Remove the factor from the factor graph
assert(fg[factorI]);
const typename FG::factor_type& removedFactor(*fg[factorI]);
assert(removedFactors.size() == jointFactorPos);
removedFactors.push_back(fg[factorI]);
fg.remove(factorI);
Index factorVarSlot = 0;
BOOST_FOREACH(const Index involvedVariable, removedFactor.keys()) {
// Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it
// that we'll fill with the slot indices for each factor that
// we're combining. Initially we put the max integer value in
// the array entry for each factor that will indicate the factor
// does not involve the variable.
static vector<Index> empty;
VariableSlots::iterator thisVarSlots = variableSlots.insert(make_pair(involvedVariable,empty)).first;
if(thisVarSlots->second.empty())
thisVarSlots->second.resize(involvedFactors.size(), numeric_limits<Index>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor " << factorI << " slot " << factorVarSlot << endl;
++ factorVarSlot;
}
++ jointFactorPos;
}
assert(variableSlots.begin()->first == j);
// Now that we know which factors and variables, and where variables
// come from and go to, create and eliminate the new joint factor.
typename FG::sharedFactor jointFactor = FG::factor_type::Combine(removedFactors, variableSlots);
assert(*jointFactor->begin() == j);
typename FG::factor_type::Conditional::shared_ptr conditional = jointFactor->eliminateFirst();
assert(conditional->key() == j);
// Add the eliminated joint factor to the partially-eliminated factor graph
fg.push_back(jointFactor);
assert(jointFactorI == fg.size()-1);
// Add the joint factor to the column index for this variable if
// it's not already added and it's not the variable we're about to
// eliminate.
if(!jointFactor->keys().empty())
columnIndex[jointFactor->front()].push_back(jointFactorI);
// Create the new node, although it's parent and children will not be
// computed yet.
// todo: use cluster factors instead of removedFactors here.
nodesToAdd.push_back(typename Node::shared_ptr(new Node(removedFactors, conditional->key(),
conditional->beginParents(), conditional->endParents())));
}
}
// Go over the collection in reverse elimination order
// and add one node for every of the n variables.
BOOST_REVERSE_FOREACH(const sharedNode& node, nodesToAdd) {
add(node); }
if(debug) this->print("Completed elimination tree: ");
}
}
/* ************************************************************************* */
} //namespace gtsam

93
inference/EliminationTree.h
View File

@ -1,93 +0,0 @@
/*
* EliminationTree.h
* Created on: Feb 4, 2010
* @Author: Kai Ni
* @Author: Frank Dellaert
* @brief: The elimination tree
*/
#pragma once
#include <set>
#include <gtsam/inference/IndexTable.h>
#include <gtsam/inference/ClusterTree.h>
namespace gtsam {
template<class> class _EliminationTreeTester;
/**
* An elimination tree (see Gilbert01bit) associated with a factor graph and an ordering
* is a cluster-tree where there is one node j for each variable, and the parent of each node
* corresponds to the first variable up the ordering in the Cholesky factor that j is connected to.
*/
template<class FG>
class EliminationTree: public ClusterTree<FG> {
public:
// In an elimination tree, the clusters are called nodes
typedef typename ClusterTree<FG>::Cluster Node;
typedef typename Node::shared_ptr sharedNode;
// we typedef the following handy list of ordered factor graphs
typedef std::pair<Index, FG> NamedGraph;
typedef std::list<NamedGraph> OrderedGraphs;
private:
/** Number of variables */
size_t nrVariables_;
/** Map from ordering index to Nodes */
typedef std::vector<sharedNode> Nodes;
Nodes nodes_;
/**
* add a factor graph fragment with given frontal key into the tree. Assumes
* parent node was already added (will throw exception if not).
*/
// void add(const FG& fg, Index key);
/**
* Add a pre-created node by computing its parent and children.
*/
void add(const sharedNode& node);
public:
/** Default constructor creates an empty elimination tree. */
EliminationTree() {}
/**
* Constructor variant 1: from an ordered list of factor graphs
* The list is supposed to be in elimination order, and for each
* eliminated variable a list of factors to be eliminated.
* This function assumes the input is correct (!) and will not check
* whether the factors refer only to the correct set of variables.
*/
// EliminationTree(const OrderedGraphs& orderedGraphs);
/**
* Constructor variant 2: given a factor graph and the elimination ordering
*/
EliminationTree(FG& fg);
friend class _EliminationTreeTester<FG>;
}; // EliminationTree
/** Class used to access private members for unit testing */
template<class FG>
struct _EliminationTreeTester {
EliminationTree<FG>& et_;
public:
_EliminationTreeTester(EliminationTree<FG>& et) : et_(et) {}
typename EliminationTree<FG>::sharedNode& root() { return et_.ClusterTree<FG>::root_; }
typename EliminationTree<FG>::Nodes& nodes() { return et_.nodes_; }
};
} // namespace gtsam

4
inference/Makefile.am
View File

@ -30,7 +30,7 @@ headers += VariableIndex.h
headers += FactorGraph.h FactorGraph-inl.h
headers += ClusterTree.h ClusterTree-inl.h
headers += JunctionTree.h JunctionTree-inl.h
headers += EliminationTree.h EliminationTree-inl.h
#headers += EliminationTree.h EliminationTree-inl.h
headers += BayesNet.h BayesNet-inl.h
headers += BayesTree.h BayesTree-inl.h
headers += ISAM.h ISAM-inl.h
@ -38,7 +38,7 @@ check_PROGRAMS += tests/testFactorGraph
check_PROGRAMS += tests/testFactorGraph
check_PROGRAMS += tests/testBayesTree
check_PROGRAMS += tests/testISAM
check_PROGRAMS += tests/testEliminationTree
#check_PROGRAMS += tests/testEliminationTree
check_PROGRAMS += tests/testClusterTree
check_PROGRAMS += tests/testJunctionTree

138
inference/tests/testEliminationTree.cpp
View File

@ -1,138 +0,0 @@
/**
* @file testEliminationTree.cpp
* @brief Unit tests for Elimination Tree
* @author Kai Ni
* @author Frank Dellaert
*/
// for operator +=
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/make_shared.hpp>
using namespace boost::assign;
#include <gtsam/CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/ClusterTree-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
using namespace std;
using namespace gtsam;
using namespace boost;
// explicit instantiation and typedef
template class EliminationTree<SymbolicFactorGraph>;
typedef EliminationTree<SymbolicFactorGraph> SymbolicEliminationTree;
/* ************************************************************************* *
* graph: f(1,2) f(1,3) f(2,5) f(3,5) f(4,5)
* tree: x1 -> x2 -> x3 -> x5 <- x4 (arrow is parent pointer)
****************************************************************************/
//TEST( EliminationTree, constructor )
//{
// Ordering ordering; ordering += "x1","x2","x3","x4","x5";
//
// /** build expected tree using constructor variant 1 */
// SymbolicEliminationTree::OrderedGraphs graphs;
// SymbolicFactorGraph c1,c2,c3,c4,c5;
// c1.push_factor("x1","x2"); c1.push_factor("x1","x3"); graphs += make_pair("x1",c1);
// c2.push_factor("x2","x5"); graphs += make_pair("x2",c2);
// c3.push_factor("x3","x5"); graphs += make_pair("x3",c3);
// c4.push_factor("x4","x5"); graphs += make_pair("x4",c4);
// graphs += make_pair("x5",c5);
// SymbolicEliminationTree expected(graphs);
//
// /** build actual tree from factor graph (variant 2) */
// SymbolicFactorGraph fg;
// fg.push_factor("x1","x2");
// fg.push_factor("x1","x3");
// fg.push_factor("x2","x5");
// fg.push_factor("x3","x5");
// fg.push_factor("x4","x5");
// SymbolicEliminationTree actual(fg, ordering);
//// GTSAM_PRINT(actual);
//
// CHECK(assert_equal<SymbolicEliminationTree>(expected, actual));
//}
/* ************************************************************************* *
* graph: f(1,2) f(1,3) f(2,5) f(3,5) f(4,5)
* tree: x1 -> x2 -> x3 -> x5 <- x4 (arrow is parent pointer)
****************************************************************************/
//TEST( EliminationTree, constructor )
//{
// Index x1=1, x2=2, x3=3, x4=4, x5=5;
// SymbolicFactorGraph fc1,fc2,fc3,fc4,fc5;
//
// fc1.push_factor(x1,x2); fc1.push_factor(x1,x3);
// list<Index> c1sep; c1sep += x2,x3;
// SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end()));
//
// fc2.push_factor(x2,x5);
// list<Index> c2sep; c2sep += x3,x5;
// SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end()));
//
// fc3.push_factor(x3,x5);
// list<Index> c3sep; c3sep += x5;
// SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end()));
//
// fc4.push_factor(x4,x5);
// list<Index> c4sep; c4sep += x5;
// SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end()));
//
// list<Index> c5sep;
// SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end()));
//
// /** build expected tree using test accessor */
// SymbolicEliminationTree expected;
// _EliminationTreeTester<SymbolicFactorGraph> expected_(expected);
// expected_.nodes().resize(6);
// expected_.root() = c5; expected_.nodes()[x5]=c5;
// c5->addChild(c4); c4->parent()=c5; expected_.nodes()[x4]=c4;
// c5->addChild(c3); c3->parent()=c5; expected_.nodes()[x3]=c3;
// c3->addChild(c2); c2->parent()=c3; expected_.nodes()[x2]=c2;
// c2->addChild(c1); c1->parent()=c2; expected_.nodes()[x1]=c1;
//
// // build actual tree from factor graph (variant 2)
// SymbolicFactorGraph fg;
// fg.push_factor(x1,x2);
// fg.push_factor(x1,x3);
// fg.push_factor(x2,x5);
// fg.push_factor(x3,x5);
// fg.push_factor(x4,x5);
// SymbolicEliminationTree actual(fg);
//// GTSAM_PRINT(actual);
//
// CHECK(assert_equal<SymbolicEliminationTree>(expected, actual));
//}
/* ************************************************************************* *
* graph: f(1,2) f(1,3) f(2,5) f(3,5) f(4,5)
* tree: x1 -> x2 -> x3 -> x5 <- x4 (arrow is parent pointer)
****************************************************************************/
TEST( EliminationTree, constructor )
{
// Create factor graph
SymbolicFactorGraph fg;
fg.push_factor(1, 2);
fg.push_factor(1, 3);
fg.push_factor(2, 5);
fg.push_factor(3, 5);
fg.push_factor(4, 5);
// Create elimination tree
SymbolicEliminationTree actual(fg);
// Check it
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

1
tests/testGaussianFactorGraph.cpp
View File

@ -743,7 +743,6 @@ TEST( GaussianFactorGraph, elimination )
fg.add(ord["x2"], Ap, b, sigma);
// Eliminate
cout << "TEST GaussianFactorGraph elimination" << endl;
GaussianBayesNet bayesNet = *Inference::Eliminate(fg);
// Check sigma