diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h deleted file mode 100644 index 0637e00fc..000000000 --- a/inference/EliminationTree-inl.h +++ /dev/null @@ -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 -#include -#include -#include - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ -// template -// void EliminationTree::add(const FG& fg, Index j) { -// sharedNode node(new Node(fg, j)); -// add(node); -// } - - /* ************************************************************************* */ - template - void EliminationTree::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::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 -// EliminationTree::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 ()); -// -// // Create a temporary map from key to ordering index -// IndexTable 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 - EliminationTree::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 > 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; fikeys().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 > 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 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 > VariableSlots; - map > 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 empty; - VariableSlots::iterator thisVarSlots = variableSlots.insert(make_pair(involvedVariable,empty)).first; - if(thisVarSlots->second.empty()) - thisVarSlots->second.resize(involvedFactors.size(), numeric_limits::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 diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h deleted file mode 100644 index 87fa164f5..000000000 --- a/inference/EliminationTree.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * EliminationTree.h - * Created on: Feb 4, 2010 - * @Author: Kai Ni - * @Author: Frank Dellaert - * @brief: The elimination tree - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -template 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 EliminationTree: public ClusterTree { - - public: - - // In an elimination tree, the clusters are called nodes - typedef typename ClusterTree::Cluster Node; - typedef typename Node::shared_ptr sharedNode; - - // we typedef the following handy list of ordered factor graphs - typedef std::pair NamedGraph; - typedef std::list OrderedGraphs; - - private: - - /** Number of variables */ - size_t nrVariables_; - - /** Map from ordering index to Nodes */ - typedef std::vector 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; - - }; // EliminationTree - - /** Class used to access private members for unit testing */ - template - struct _EliminationTreeTester { - - EliminationTree& et_; - - public: - _EliminationTreeTester(EliminationTree& et) : et_(et) {} - - typename EliminationTree::sharedNode& root() { return et_.ClusterTree::root_; } - typename EliminationTree::Nodes& nodes() { return et_.nodes_; } - }; - -} // namespace gtsam diff --git a/inference/Makefile.am b/inference/Makefile.am index 84442a872..618e874d8 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -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 diff --git a/inference/tests/testEliminationTree.cpp b/inference/tests/testEliminationTree.cpp deleted file mode 100644 index 9a9465bf8..000000000 --- a/inference/tests/testEliminationTree.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/** - * @file testEliminationTree.cpp - * @brief Unit tests for Elimination Tree - * @author Kai Ni - * @author Frank Dellaert - */ - -// for operator += -#include -#include -#include -using namespace boost::assign; - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost; - -// explicit instantiation and typedef -template class EliminationTree; -typedef EliminationTree 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(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 c1sep; c1sep += x2,x3; -// SymbolicEliminationTree::sharedNode c1(new SymbolicEliminationTree::Node(fc1, x1, c1sep.begin(), c1sep.end())); -// -// fc2.push_factor(x2,x5); -// list c2sep; c2sep += x3,x5; -// SymbolicEliminationTree::sharedNode c2(new SymbolicEliminationTree::Node(fc2, x2, c2sep.begin(), c2sep.end())); -// -// fc3.push_factor(x3,x5); -// list c3sep; c3sep += x5; -// SymbolicEliminationTree::sharedNode c3(new SymbolicEliminationTree::Node(fc3, x3, c3sep.begin(), c3sep.end())); -// -// fc4.push_factor(x4,x5); -// list c4sep; c4sep += x5; -// SymbolicEliminationTree::sharedNode c4(new SymbolicEliminationTree::Node(fc4, x4, c4sep.begin(), c4sep.end())); -// -// list c5sep; -// SymbolicEliminationTree::sharedNode c5(new SymbolicEliminationTree::Node(fc5, x5, c5sep.begin(), c5sep.end())); -// -// /** build expected tree using test accessor */ -// SymbolicEliminationTree expected; -// _EliminationTreeTester 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(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); -} -/* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index eafd29003..83c1383a7 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -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