From 7d384488e39de5cbd65b79f30f6dd5b4b7904bde Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Sun, 22 Nov 2009 23:35:13 +0000 Subject: [PATCH] BayesTree::update generalized and moved from testIncremental to BayesTree; testIncremental removed --- .cproject | 38 +++++------- cpp/BayesTree-inl.h | 41 +++++++++++++ cpp/BayesTree.h | 6 ++ cpp/Makefile.am | 4 +- cpp/testBayesTree.cpp | 54 +++++++++++++++++ cpp/testIncremental.cpp | 124 ---------------------------------------- 6 files changed, 116 insertions(+), 151 deletions(-) delete mode 100644 cpp/testIncremental.cpp diff --git a/.cproject b/.cproject index 5633bb6f1..3811dd5c4 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -324,7 +322,6 @@ make - testSimpleCamera.run true true @@ -340,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -347,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -355,6 +352,7 @@ make + testGaussianConditional.run true true @@ -362,7 +360,6 @@ make - testPose2.run true true @@ -370,7 +367,6 @@ make - testRot3.run true true @@ -378,6 +374,7 @@ make + testNonlinearOptimizer.run true true @@ -385,7 +382,6 @@ make - testGaussianFactor.run true true @@ -393,7 +389,6 @@ make - testGaussianFactorGraph.run true true @@ -401,6 +396,7 @@ make + testNonlinearFactorGraph.run true true @@ -408,7 +404,6 @@ make - testPose3.run true true @@ -416,6 +411,7 @@ make + testVectorConfig.run true true @@ -423,6 +419,7 @@ make + testPoint2.run true true @@ -430,7 +427,6 @@ make - testNonlinearFactor.run true true @@ -438,7 +434,6 @@ make - timeGaussianFactor.run true true @@ -446,7 +441,6 @@ make - timeGaussianFactorGraph.run true true @@ -454,7 +448,6 @@ make - testGaussianBayesNet.run true true @@ -462,6 +455,7 @@ make + testBayesTree.run true false @@ -469,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -477,6 +470,7 @@ make + testSymbolicFactorGraph.run true false @@ -484,7 +478,6 @@ make - testVector.run true true @@ -492,21 +485,14 @@ make - testMatrix.run true true true - -make -testIncremental.run -true -true -true - make + testVSLAMGraph.run true true @@ -514,6 +500,7 @@ make + testNonlinearEquality.run true true @@ -521,7 +508,6 @@ make - testSQP.run true true @@ -529,6 +515,7 @@ make + testNonlinearConstraint.run true true @@ -536,6 +523,7 @@ make + install true true @@ -543,6 +531,7 @@ make + clean true true @@ -550,6 +539,7 @@ make + check true true diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index e6f882362..dabb1961d 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -2,6 +2,8 @@ * @file BayesTree.cpp * @brief Bayes Tree is a tree of cliques of a Bayes Chain * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila */ #include @@ -417,6 +419,45 @@ namespace gtsam { } /* ************************************************************************* */ + template + template + void BayesTree::update(const FactorGraph& newFactors) { + // Remove the contaminated part of the Bayes tree + FactorGraph factors; + typename BayesTree::Cliques orphans; + BOOST_FOREACH(boost::shared_ptr factor, newFactors) { + + FactorGraph factors1; + typename BayesTree::Cliques orphans1; + boost::tie(factors1, orphans1) = removeTop(factor); + + factors.push_back(factors1); + orphans.splice (orphans.begin(), orphans1); + } + + // add the factors themselves + factors.push_back(newFactors); + + // create an ordering for the new and contaminated factors + Ordering ordering = factors.getOrdering(); + + // eliminate into a Bayes net + BayesNet bayesNet = eliminate(factors,ordering); + + // insert conditionals back in, straight into the topless bayesTree + typename BayesNet::const_reverse_iterator rit; + for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) + insert(*rit); + + // add orphans to the bottom of the new tree + BOOST_FOREACH(typename BayesTree::sharedClique orphan, orphans) { + string key = orphan->separator_.front(); // todo: assumes there is a separator... + typename BayesTree::sharedClique parent = (*this)[key]; + parent->children_ += orphan; + } + + } + /* ************************************************************************* */ } /// namespace gtsam diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 4553e27bf..39e57f9c3 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -177,6 +177,12 @@ namespace gtsam { template std::pair, Cliques> removeTop(const boost::shared_ptr& newFactor); + /** + * iSAM. + */ + template + void update(const FactorGraph& newFactors); + }; // BayesTree } /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 862a1b1ab..3398754a6 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -64,15 +64,13 @@ example = smallExample.cpp headers += inference.h inference-inl.h headers += FactorGraph.h FactorGraph-inl.h headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h -check_PROGRAMS += testFactorgraph testBayesTree testInference testIncremental +check_PROGRAMS += testFactorgraph testBayesTree testInference testFactorgraph_SOURCES = testFactorgraph.cpp testBayesTree_SOURCES = $(example) testBayesTree.cpp testInference_SOURCES = $(example) testInference.cpp -testIncremental_SOURCES = $(example) testIncremental.cpp testFactorgraph_LDADD = libgtsam.la testBayesTree_LDADD = libgtsam.la testInference_LDADD = libgtsam.la -testIncremental_LDADD = libgtsam.la # Symbolic Inference headers += SymbolicConditional.h diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index b1dd31f95..8f4483a6d 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -2,6 +2,8 @@ * @file testBayesTree.cpp * @brief Unit tests for Bayes Tree * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila */ #include // for operator += @@ -435,6 +437,58 @@ TEST( BayesTree, removeTop ) CHECK(assert_equal(expectedOrphans2, orphans)); } + +/* ************************************************************************* */ +TEST( BayesTree, iSAM ) +{ + // Conditionals for ASIA example from the tutorial with A and D evidence + SymbolicConditional::shared_ptr + B(new SymbolicConditional("B")), + L(new SymbolicConditional("L", "B")), + E(new SymbolicConditional("E", "B", "L")), + S(new SymbolicConditional("S", "L", "B")), + T(new SymbolicConditional("T", "E", "L")), + X(new SymbolicConditional("X", "E")); + + // Create using insert + SymbolicBayesTree bayesTree; + bayesTree.insert(B); + bayesTree.insert(L); + bayesTree.insert(E); + bayesTree.insert(S); + bayesTree.insert(T); + bayesTree.insert(X); + + // Now we modify the Bayes tree by inserting a new factor over B and S + + // New conditionals in modified top of the tree + SymbolicConditional::shared_ptr + S_(new SymbolicConditional("S")), + L_(new SymbolicConditional("L", "S")), + E_(new SymbolicConditional("E", "L", "S")), + B_(new SymbolicConditional("B", "E", "L", "S")); + + // Create expected Bayes tree + SymbolicBayesTree expected; + expected.insert(S_); + expected.insert(L_); + expected.insert(E_); + expected.insert(B_); + expected.insert(T); + expected.insert(X); + + // create new factors to be inserted + SymbolicFactorGraph factorGraph; + factorGraph.push_factor("B","S"); + factorGraph.push_factor("B"); + + // do incremental inference + bayesTree.update(factorGraph); + + // Check whether the same + CHECK(assert_equal(expected,bayesTree)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testIncremental.cpp b/cpp/testIncremental.cpp deleted file mode 100644 index 853a2c177..000000000 --- a/cpp/testIncremental.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/** - * @file testIncremental.cpp - * @brief Unit tests for graph-based iSAM - * @author Michael Kaess - * @author Viorela Ila - * @author Frank Dellaert - */ - -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include "SymbolicBayesNet.h" -#include "SymbolicFactorGraph.h" -#include "GaussianBayesNet.h" -#include "Ordering.h" -#include "BayesTree-inl.h" -#include "smallExample.h" - -using namespace std; -using namespace gtsam; - -typedef BayesTree SymbolicBayesTree; -typedef BayesTree GaussianBayesTree; - -/* ************************************************************************* */ - -void update(SymbolicBayesTree& bayesTree, const FactorGraph& newFactors) { - - // Remove the contaminated part of the Bayes tree - FactorGraph factors; - SymbolicBayesTree::Cliques orphans; - BOOST_FOREACH(boost::shared_ptr factor, newFactors) { - - FactorGraph factors1; - SymbolicBayesTree::Cliques orphans1; - boost::tie(factors1, orphans1) = bayesTree.removeTop(factor); - - factors.push_back(factors1); - orphans.splice (orphans.begin(), orphans1); - } - - // add the factors themselves - factors.push_back(newFactors); - - // create an ordering for the new and contaminated factors - Ordering ordering = factors.getOrdering(); - - // eliminate into a Bayes net - SymbolicBayesNet bayesNet = eliminate(factors,ordering); - - // insert conditionals back in, straight into the topless bayesTree - SymbolicBayesNet::const_reverse_iterator rit; - for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - bayesTree.insert(*rit); - - // add orphans to the bottom of the new tree - BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) { - string key = orphan->separator_.front(); // todo: assumes there is a separator... - SymbolicBayesTree::sharedClique parent = bayesTree[key]; - parent->children_ += orphan; - } - -} - -/* ************************************************************************* */ -TEST( BayesTree, iSAM ) -{ - // Conditionals for ASIA example from the tutorial with A and D evidence - SymbolicConditional::shared_ptr - B(new SymbolicConditional("B")), - L(new SymbolicConditional("L", "B")), - E(new SymbolicConditional("E", "B", "L")), - S(new SymbolicConditional("S", "L", "B")), - T(new SymbolicConditional("T", "E", "L")), - X(new SymbolicConditional("X", "E")); - - // Create using insert - SymbolicBayesTree bayesTree; - bayesTree.insert(B); - bayesTree.insert(L); - bayesTree.insert(E); - bayesTree.insert(S); - bayesTree.insert(T); - bayesTree.insert(X); - - // Now we modify the Bayes tree by inserting a new factor over B and S - - // New conditionals in modified top of the tree - SymbolicConditional::shared_ptr - S_(new SymbolicConditional("S")), - L_(new SymbolicConditional("L", "S")), - E_(new SymbolicConditional("E", "L", "S")), - B_(new SymbolicConditional("B", "E", "L", "S")); - - // Create expected Bayes tree - SymbolicBayesTree expected; - expected.insert(S_); - expected.insert(L_); - expected.insert(E_); - expected.insert(B_); - expected.insert(T); - expected.insert(X); - - // create new factors to be inserted - SymbolicFactorGraph factorGraph; - factorGraph.push_factor("B","S"); - factorGraph.push_factor("B"); - - // do incremental inference - update(bayesTree, factorGraph); - - // Check whether the same - CHECK(assert_equal(expected,bayesTree)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */