diff --git a/.cproject b/.cproject index 2f30bd246..77e165a14 100644 --- a/.cproject +++ b/.cproject @@ -322,14 +322,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -356,6 +348,7 @@ make + tests/testBayesTree.run true false @@ -363,6 +356,7 @@ make + testBinaryBayesNet.run true false @@ -410,6 +404,7 @@ make + testSymbolicBayesNet.run true false @@ -417,6 +412,7 @@ make + tests/testSymbolicFactor.run true false @@ -424,6 +420,7 @@ make + testSymbolicFactorGraph.run true false @@ -439,11 +436,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -478,7 +484,6 @@ make - testGraph.run true false @@ -574,7 +579,6 @@ make - testInference.run true false @@ -582,7 +586,6 @@ make - testGaussianBayesNet.run true false @@ -590,7 +593,6 @@ make - testGaussianFactor.run true false @@ -598,7 +600,6 @@ make - testJunctionTree.run true false @@ -606,7 +607,6 @@ make - testSymbolicBayesNet.run true false @@ -614,7 +614,6 @@ make - testSymbolicFactorGraph.run true false @@ -822,6 +821,7 @@ make + testGaussianISAM.run true true @@ -829,7 +829,6 @@ make - testSymbolicFactorGraph.run true true @@ -933,6 +932,7 @@ make + testErrors.run true false @@ -1084,7 +1084,6 @@ make - tests/testEliminationTree.run true true @@ -1108,12 +1107,19 @@ make - tests/testConditional.run true true true + + make + + tests/testSymbolicFactorGraph.run + true + true + true + make -j2 @@ -1276,7 +1282,6 @@ make - testSimulated2DOriented.run true false @@ -1316,7 +1321,6 @@ make - testSimulated2D.run true false @@ -1324,7 +1328,6 @@ make - testSimulated3D.run true false @@ -1372,7 +1375,6 @@ make - tests/testGaussianISAM2 true false @@ -1394,6 +1396,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -1490,86 +1572,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -1924,14 +1926,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1958,6 +1952,7 @@ make + tests/testBayesTree.run true false @@ -1965,6 +1960,7 @@ make + testBinaryBayesNet.run true false @@ -2012,6 +2008,7 @@ make + testSymbolicBayesNet.run true false @@ -2019,6 +2016,7 @@ make + tests/testSymbolicFactor.run true false @@ -2026,6 +2024,7 @@ make + testSymbolicFactorGraph.run true false @@ -2041,11 +2040,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2080,7 +2088,6 @@ make - testGraph.run true false @@ -2176,7 +2183,6 @@ make - testInference.run true false @@ -2184,7 +2190,6 @@ make - testGaussianBayesNet.run true false @@ -2192,7 +2197,6 @@ make - testGaussianFactor.run true false @@ -2200,7 +2204,6 @@ make - testJunctionTree.run true false @@ -2208,7 +2211,6 @@ make - testSymbolicBayesNet.run true false @@ -2216,7 +2218,6 @@ make - testSymbolicFactorGraph.run true false @@ -2424,6 +2425,7 @@ make + testGaussianISAM.run true true @@ -2431,7 +2433,6 @@ make - testSymbolicFactorGraph.run true true @@ -2535,6 +2536,7 @@ make + testErrors.run true false @@ -2686,7 +2688,6 @@ make - tests/testEliminationTree.run true true @@ -2710,12 +2711,19 @@ make - tests/testConditional.run true true true + + make + + tests/testSymbolicFactorGraph.run + true + true + true + make -j2 @@ -2878,7 +2886,6 @@ make - testSimulated2DOriented.run true false @@ -2918,7 +2925,6 @@ make - testSimulated2D.run true false @@ -2926,7 +2932,6 @@ make - testSimulated3D.run true false @@ -2974,7 +2979,6 @@ make - tests/testGaussianISAM2 true false @@ -2996,6 +3000,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -3092,86 +3176,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 diff --git a/inference/tests/testSymbolicFactorGraph.cpp b/inference/tests/testSymbolicFactorGraph.cpp index 3f1384a87..5039c8b82 100644 --- a/inference/tests/testSymbolicFactorGraph.cpp +++ b/inference/tests/testSymbolicFactorGraph.cpp @@ -18,14 +18,14 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -static const Index vx2=0; -static const Index vx1=1; -static const Index vl1=2; +static const Index vx2 = 0; +static const Index vx1 = 1; +static const Index vl1 = 2; /* ************************************************************************* */ -TEST( SymbolicFactorGraph, eliminate2 ) +TEST( SymbolicFactorGraph, EliminateOne ) { - // create a test graph + // create a test graph SymbolicFactorGraph fg; fg.push_factor(vx2, vx1); @@ -43,24 +43,24 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) { // create expected factor graph SymbolicFactorGraph expected; - expected.push_factor(vx2,vx1,vl1); - expected.push_factor(vx1,vl1); + expected.push_factor(vx2, vx1, vl1); + expected.push_factor(vx1, vl1); expected.push_factor(vx1); - // create Bayes Net - Conditional::shared_ptr x2(new Conditional(vx2, vx1, vl1)); - Conditional::shared_ptr l1(new Conditional(vx1, vl1)); - Conditional::shared_ptr x1(new Conditional(vx1)); + // create Bayes Net + Conditional::shared_ptr x2(new Conditional(vx2, vx1, vl1)); + Conditional::shared_ptr l1(new Conditional(vx1, vl1)); + Conditional::shared_ptr x1(new Conditional(vx1)); - BayesNet bayesNet; - bayesNet.push_back(x2); - bayesNet.push_back(l1); - bayesNet.push_back(x1); + BayesNet bayesNet; + bayesNet.push_back(x2); + bayesNet.push_back(l1); + bayesNet.push_back(x1); - // create actual factor graph from a Bayes Net + // create actual factor graph from a Bayes Net SymbolicFactorGraph actual(bayesNet); - CHECK(assert_equal((SymbolicFactorGraph)expected,actual)); + CHECK(assert_equal((SymbolicFactorGraph)expected,actual)); } /* ************************************************************************* */ @@ -70,18 +70,18 @@ TEST( SymbolicFactorGraph, push_back ) SymbolicFactorGraph fg1, fg2, expected; fg1.push_factor(vx1); - fg1.push_factor(vx2,vx1); + fg1.push_factor(vx2, vx1); - fg2.push_factor(vx1,vl1); - fg2.push_factor(vx2,vl1); + fg2.push_factor(vx1, vl1); + fg2.push_factor(vx2, vl1); expected.push_factor(vx1); - expected.push_factor(vx2,vx1); - expected.push_factor(vx1,vl1); - expected.push_factor(vx2,vl1); + expected.push_factor(vx2, vx1); + expected.push_factor(vx1, vl1); + expected.push_factor(vx2, vl1); // combine - SymbolicFactorGraph actual = combine(fg1,fg2); + SymbolicFactorGraph actual = combine(fg1, fg2); CHECK(assert_equal(expected, actual)); // combine using push_back @@ -89,6 +89,301 @@ TEST( SymbolicFactorGraph, push_back ) CHECK(assert_equal(expected, fg1)); } +/* ************************************************************************* */ + +/** + * An elimination tree is a tree of factors + */ +class ETree: public Testable { + +public: + + typedef boost::shared_ptr sharedFactor; + typedef boost::shared_ptr shared_ptr; + +private: + + Index key_; /** index associated with root */ + list factors_; /** factors associated with root */ + list subTrees_; /** sub-trees */ + + typedef pair Result; + + /** + * Recursive routine that eliminates the factors arranged in an elimination tree + */ + Result eliminate_() const { + + SymbolicBayesNet bn; + + set separator; + + // loop over all factors associated with root + // and set-union their keys to a separator + BOOST_FOREACH(const sharedFactor& factor, factors_) + BOOST_FOREACH(Index key, *factor) + if (key != key_) separator.insert(key); + + // for all children, eliminate into Bayes net + BOOST_FOREACH(const shared_ptr& child, subTrees_) + { + Result result = child->eliminate_(); + bn.push_back(result.first); + BOOST_FOREACH(Index key, result.second) + if (key != key_) separator.insert(key); + } + + // Make the conditional from the key and separator, and insert it in Bayes net + vector parents; + std::copy(separator.begin(), separator.end(), back_inserter(parents)); + Conditional::shared_ptr conditional(new Conditional(key_, parents)); + bn.push_back(conditional); + + // now create the new factor from separator to return to caller + Factor newFactor(separator.begin(), separator.end()); + return Result(bn, newFactor); + } + +public: + + /** default constructor */ + ETree(Index key = 0) : + key_(key) { + } + + /** add a factor */ + void add(const sharedFactor& factor) { + factors_.push_back(factor); + } + + /** add a subtree */ + void add(const shared_ptr& child) { + subTrees_.push_back(child); + } + + void print(const std::string& name) const { + cout << name << " (" << key_ << ")" << endl; + BOOST_FOREACH(const sharedFactor& factor, factors_) + factor->print(name + " "); + BOOST_FOREACH(const shared_ptr& child, subTrees_) + child->print(name + " "); + } + + bool equals(const ETree& expected, double tol) const { + // todo + return false; + } + + /** + * Eliminate the factors to a Bayes Net + */ + SymbolicBayesNet eliminate() const { + + // call recursive routine + Result result = eliminate_(); + return result.first; + } + +}; + +// build hardcoded tree +ETree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { + + ETree::shared_ptr leaf0(new ETree); + leaf0->add(fg[0]); + leaf0->add(fg[1]); + + ETree::shared_ptr node1(new ETree(1)); + node1->add(fg[2]); + node1->add(leaf0); + + ETree::shared_ptr node2(new ETree(2)); + node2->add(fg[3]); + node2->add(node1); + + ETree::shared_ptr leaf3(new ETree(3)); + leaf3->add(fg[4]); + + ETree::shared_ptr etree(new ETree(4)); + etree->add(leaf3); + etree->add(node2); + + return etree; +} + +typedef size_t RowIndex; +typedef vector > StructA; +typedef vector > optionalIndices; + +/** + * Gilbert01bit algorithm in Figure 2.2 + */ +optionalIndices buildETree(const StructA& structA) { + + // todo: get n + size_t n = 5; + optionalIndices parent(n); + + // todo: get m + size_t m = 5; + optionalIndices prev_col(m); + + // for column j \in 1 to n do + for (Index j = 0; j < n; j++) { + // for row i \in Struct[A*j] do + BOOST_FOREACH(RowIndex i, structA[j]) + { + if (prev_col[i]) { + Index k = *(prev_col[i]); + // find root r of the current tree that contains k + Index r = k; + while (parent[r]) + r = *parent[r]; + if (r != j) parent[r].reset(j); + } + prev_col[i].reset(j); + } + } + + return parent; +} + +/** + * TODO: Build StructA from factor graph + */ +StructA createStructA(const SymbolicFactorGraph& fg) { + + StructA structA; + + // hardcode for now + list list0; + list0 += 0, 1; + structA.push_back(list0); + list list1; + list1 += 0, 2; + structA.push_back(list1); + list list2; + list2 += 1, 3; + structA.push_back(list2); + list list3; + list3 += 4; + structA.push_back(list3); + list list4; + list4 += 2, 3, 4; + structA.push_back(list4); + + return structA; +} + +/** + * Build ETree from factor graph and parent indices + */ +ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg, const optionalIndices& parent) { + + // todo: get n + size_t n = 5; + + // Create tree structure + vector trees(n); + for (Index k = 1; k <= n; k++) { + Index j = n - k; + trees[j].reset(new ETree(j)); + if (parent[j]) trees[*parent[j]]->add(trees[j]); + } + + // Hang factors in right places + BOOST_FOREACH(const ETree::sharedFactor& factor, fg) + { + Index j = factor->front(); + trees[j]->add(factor); + } + + return trees[n - 1]; +} + +/* ************************************************************************* */ +// Test of elimination tree creation +// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) +/* ************************************************************************* */ +/** + * Build ETree from factor graph + */ +ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg) { + + // create vector of factor indices + StructA structA = createStructA(fg); + + // call Gilbert01bit algorithm + optionalIndices parent = buildETree(structA); + + // Build ETree from factor graph and parent indices + return buildETree(fg, parent); +} + +TEST( ETree, buildETree ) +{ + // create example factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + ETree::shared_ptr expected = buildHardcodedTree(fg); + + // Build from factor graph + ETree::shared_ptr actual = buildETree(fg); + + // todo: CHECK(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +// Test to drive elimination tree development +// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) +/* ************************************************************************* */ + +/** + * Eliminate factor graph + */ +SymbolicBayesNet eliminate(const SymbolicFactorGraph& fg) { + + // build etree + ETree::shared_ptr etree = buildETree(fg); + + return etree->eliminate(); +} + +TEST( SymbolicFactorGraph, eliminate ) +{ + // create expected Chordal bayes Net + Conditional::shared_ptr c0(new Conditional(0, 1, 2)); + Conditional::shared_ptr c1(new Conditional(1, 2, 4)); + Conditional::shared_ptr c2(new Conditional(2, 4)); + Conditional::shared_ptr c3(new Conditional(3, 4)); + Conditional::shared_ptr c4(new Conditional(4)); + + SymbolicBayesNet expected; + expected.push_back(c3); + expected.push_back(c0); + expected.push_back(c1); + expected.push_back(c2); + expected.push_back(c4); + + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + SymbolicBayesNet actual = eliminate(fg); + + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index ca307f5be..3372a0aeb 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -139,41 +139,6 @@ TEST( SymbolicFactorGraph, eliminate ) CHECK(assert_equal(expected,actual)); } -/* ************************************************************************* */ -// Test to drive elimination tree development -// Uses example originally created in testEliminationTree -// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, eliminate2 ) -{ - // create expected Chordal bayes Net - Conditional::shared_ptr c0(new Conditional(0, 1, 2)); - Conditional::shared_ptr c1(new Conditional(1, 2, 4)); - Conditional::shared_ptr c2(new Conditional(2, 4)); - Conditional::shared_ptr c3(new Conditional(3, 4)); - Conditional::shared_ptr c4(new Conditional(4)); - - SymbolicBayesNet expected; - expected.push_back(c0); - expected.push_back(c1); - expected.push_back(c2); - expected.push_back(c3); - expected.push_back(c4); - - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - // eliminate it - SymbolicBayesNet actual = *Inference::Eliminate(fg); - - CHECK(assert_equal(expected,actual)); -} - /* ************************************************************************* */ int main() { TestResult tr;