From 5dfd1921e1a292c515e9cf90e9b3f8bd396d7300 Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Sun, 27 Dec 2009 23:15:36 +0000 Subject: [PATCH] template nightmare resolved, all tests working --- .cproject | 57 +++---- cpp/BayesTree-inl.h | 56 +------ cpp/BayesTree.h | 31 +--- cpp/GaussianBayesTree.h | 33 ---- ...GaussianBayesTree.cpp => GaussianISAM.cpp} | 23 ++- cpp/GaussianISAM.h | 24 +++ cpp/ISAM-inl.h | 80 ++++++++++ cpp/ISAM.h | 54 +++++++ cpp/Makefile.am | 18 ++- cpp/testBayesTree.cpp | 118 +++----------- ...sianBayesTree.cpp => testGaussianISAM.cpp} | 146 +++++++++--------- cpp/testISAM.cpp | 143 +++++++++++++++++ 12 files changed, 453 insertions(+), 330 deletions(-) delete mode 100644 cpp/GaussianBayesTree.h rename cpp/{GaussianBayesTree.cpp => GaussianISAM.cpp} (60%) create mode 100644 cpp/GaussianISAM.h create mode 100644 cpp/ISAM-inl.h create mode 100644 cpp/ISAM.h rename cpp/{testGaussianBayesTree.cpp => testGaussianISAM.cpp} (89%) create mode 100644 cpp/testISAM.cpp diff --git a/.cproject b/.cproject index aa3eba21d..26d549d35 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,7 +485,6 @@ make - testMatrix.run true true @@ -500,6 +492,7 @@ make + testVSLAMGraph.run true true @@ -507,6 +500,7 @@ make + testNonlinearEquality.run true true @@ -514,7 +508,6 @@ make - testSQP.run true true @@ -522,6 +515,7 @@ make + testNonlinearConstraint.run true true @@ -529,7 +523,6 @@ make - testSQPOptimizer.run true true @@ -537,7 +530,6 @@ make - testVSLAMConfig.run true true @@ -545,7 +537,6 @@ make - testControlConfig.run true true @@ -553,7 +544,6 @@ make - testControlPoint.run true true @@ -561,6 +551,7 @@ make + testControlGraph.run true true @@ -568,7 +559,6 @@ make - testOrdering.run true true @@ -576,6 +566,7 @@ make + testPose2Constraint.run true true @@ -583,22 +574,21 @@ make - testRot2.run true true true - + make - -testGaussianBayesTree.run +testGaussianISAM.run true true true make + testPose3Factor.run true true @@ -606,6 +596,7 @@ make + testUrbanGraph.run true true @@ -613,6 +604,7 @@ make + testUrbanConfig.run true true @@ -620,6 +612,7 @@ make + testUrbanMeasurement.run true true @@ -627,14 +620,22 @@ make - testUrbanOdometry.run true true true + +make + +testISAM.run +true +true +true + make + install true true @@ -642,6 +643,7 @@ make + clean true true @@ -649,6 +651,7 @@ make + check true true diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index fa72de202..58f489520 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -371,7 +371,7 @@ namespace gtsam { template template pair, typename BayesTree::Cliques> - ISAM::removePath(sharedClique clique) { + BayesTree::removePath(sharedClique clique) { FactorGraph factors; Cliques orphans; @@ -403,7 +403,7 @@ namespace gtsam { // TODO: add to factors and orphans template template - void ISAM::removeTop(const boost::shared_ptr& newFactor, + void BayesTree::removeTop(const boost::shared_ptr& newFactor, FactorGraph &factors, typename BayesTree::Cliques& orphans) { // process each key of the new factor @@ -429,7 +429,7 @@ namespace gtsam { template template pair, typename BayesTree::Cliques> - ISAM::removeTop(const FactorGraph& newFactors) { + BayesTree::removeTop(const FactorGraph& newFactors) { // Remove the contaminated part of the Bayes tree FactorGraph factors; Cliques orphans; @@ -440,56 +440,6 @@ namespace gtsam { return make_pair(factors,orphans); } - /* ************************************************************************* */ - template - template - void ISAM::update_internal(const FactorGraph& newFactors, Cliques& orphans) { - - // Remove the contaminated part of the Bayes tree - FactorGraph factors; - boost::tie(factors, orphans) = this->removeTop(newFactors); - - // add the factors themselves - factors.push_back(newFactors); - - // create an ordering for the new and contaminated factors - Ordering ordering; - if (true) { - ordering = factors.getOrdering(); - } else { - list keys = factors.keys(); - keys.sort(); // todo: correct sorting order? - ordering = keys; - } - - // 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 ) - this->insert(*rit); - - int count = 0; - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - - string key = orphan->separator_.front(); - sharedClique parent = (*this)[key]; - - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - - } - - template - template - void ISAM::update(const FactorGraph& newFactors) { - Cliques orphans; - this->update_internal(newFactors, orphans); - } - /* ************************************************************************* */ } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index e5baa73aa..349e889b6 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -165,27 +165,6 @@ namespace gtsam { template BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; - }; // BayesTree - - template - class ISAM: public BayesTree { - - public: - - /** Create an empty Bayes Tree */ - ISAM() : BayesTree() {} - - /** Create a Bayes Tree from a Bayes Net */ - ISAM(const BayesNet& bayesNet) : BayesTree(bayesNet) {} - - /** Destructor */ - virtual ~ISAM() { - } - - typedef typename BayesTree::sharedClique sharedClique; - - typedef typename BayesTree::Cliques Cliques; - /** * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. @@ -210,14 +189,6 @@ namespace gtsam { std::pair, Cliques> removeTop(const FactorGraph& newFactors); - /** - * iSAM. (_internal provides access to list of orphans for drawing purposes) - */ - template - void update_internal(const FactorGraph& newFactors, Cliques& orphans); - template - void update(const FactorGraph& newFactors); - - }; // ISAM + }; // BayesTree } /// namespace gtsam diff --git a/cpp/GaussianBayesTree.h b/cpp/GaussianBayesTree.h deleted file mode 100644 index 86b3d6baa..000000000 --- a/cpp/GaussianBayesTree.h +++ /dev/null @@ -1,33 +0,0 @@ -/** - * @file GaussianBayesTree - * @brief Bayes Tree is a tree of cliques of a Bayes Chain - * @author Michael Kaess - */ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "Testable.h" -#include "BayesTree.h" -#include "VectorConfig.h" -#include "GaussianConditional.h" - -namespace gtsam { - - typedef BayesTree GaussianBayesTree; - - // recursively optimize this conditional and all subtrees - void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result); - - // optimize the BayesTree, starting from the root - VectorConfig optimize(const GaussianBayesTree& bayesTree); - -}/// namespace gtsam diff --git a/cpp/GaussianBayesTree.cpp b/cpp/GaussianISAM.cpp similarity index 60% rename from cpp/GaussianBayesTree.cpp rename to cpp/GaussianISAM.cpp index 88af4543e..ed3f37bef 100644 --- a/cpp/GaussianBayesTree.cpp +++ b/cpp/GaussianISAM.cpp @@ -1,41 +1,38 @@ /** - * @file GaussianBayesTree - * @brief Bayes Tree is a tree of cliques of a Bayes Chain + * @file GaussianISAM + * @brief * @author Michael Kaess */ -#include - -#include "GaussianBayesTree.h" -#include "VectorConfig.h" +#include "GaussianISAM.h" using namespace std; using namespace gtsam; // Explicitly instantiate so we don't have to include everywhere -#include "BayesTree-inl.h" -template class BayesTree; +#include "ISAM-inl.h" +template class ISAM; namespace gtsam { /* ************************************************************************* */ -void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) { +void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) { // parents are assumed to already be solved and available in result - GaussianBayesTree::Clique::const_reverse_iterator it; + GaussianISAM::Clique::const_reverse_iterator it; for (it = clique->rbegin(); it!=clique->rend(); it++) { GaussianConditional::shared_ptr cg = *it; Vector x = cg->solve(result); // Solve for that variable result.insert(cg->key(), x); // store result in partial solution } - BOOST_FOREACH(GaussianBayesTree::sharedClique child, clique->children_) { -// list::const_iterator child; + BOOST_FOREACH(GaussianISAM::sharedClique child, clique->children_) { +// list::const_iterator child; // for (child = clique->children_.begin(); child != clique->children_.end(); child++) { optimize(child, result); } } /* ************************************************************************* */ -VectorConfig optimize(const GaussianBayesTree& bayesTree) { +VectorConfig optimize(const GaussianISAM& bayesTree) { VectorConfig result; // starting from the root, call optimize on each conditional optimize(bayesTree.root(), result); diff --git a/cpp/GaussianISAM.h b/cpp/GaussianISAM.h new file mode 100644 index 000000000..63e533d27 --- /dev/null +++ b/cpp/GaussianISAM.h @@ -0,0 +1,24 @@ +/** + * @file GaussianISAM + * @brief + * @author Michael Kaess + */ + +// \callgraph + +#pragma once + +#include "ISAM.h" +#include "GaussianConditional.h" + +namespace gtsam { + + typedef ISAM GaussianISAM; + + // recursively optimize this conditional and all subtrees + void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result); + + // optimize the BayesTree, starting from the root + VectorConfig optimize(const GaussianISAM& bayesTree); + +}/// namespace gtsam diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h new file mode 100644 index 000000000..8bd24612c --- /dev/null +++ b/cpp/ISAM-inl.h @@ -0,0 +1,80 @@ +/** + * @file ISAM-inl.h + * @brief Incremental update functionality (iSAM) for BayesTree. + * @author Michael Kaess + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include "Conditional.h" +#include "BayesTree-inl.h" +#include "ISAM.h" + +namespace gtsam { + + using namespace std; + + /** Create an empty Bayes Tree */ + template + ISAM::ISAM() : BayesTree() {} + + /** Create a Bayes Tree from a Bayes Net */ + template + ISAM::ISAM(const BayesNet& bayesNet) : BayesTree(bayesNet) {} + + /* ************************************************************************* */ + template + template + void ISAM::update_internal(const FactorGraph& newFactors, Cliques& orphans) { + + // Remove the contaminated part of the Bayes tree + FactorGraph factors; + boost::tie(factors, orphans) = removeTop(newFactors); + + // add the factors themselves + factors.push_back(newFactors); + + // create an ordering for the new and contaminated factors + Ordering ordering; + if (true) { + ordering = factors.getOrdering(); + } else { + list keys = factors.keys(); + keys.sort(); // todo: correct sorting order? + ordering = keys; + } + + // 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 ) + this->insert(*rit); + + int count = 0; + // add orphans to the bottom of the new tree + BOOST_FOREACH(sharedClique orphan, orphans) { + + string key = orphan->separator_.front(); + sharedClique parent = (*this)[key]; + + parent->children_ += orphan; + orphan->parent_ = parent; // set new parent! + } + + } + + template + template + void ISAM::update(const FactorGraph& newFactors) { + Cliques orphans; + this->update_internal(newFactors, orphans); + } + +/* ************************************************************************* */ + +} +/// namespace gtsam diff --git a/cpp/ISAM.h b/cpp/ISAM.h new file mode 100644 index 000000000..de19dfa04 --- /dev/null +++ b/cpp/ISAM.h @@ -0,0 +1,54 @@ +/** + * @file ISAM.h + * @brief Incremental update functionality (iSAM) for BayesTree. + * @author Michael Kaess + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "Testable.h" +#include "FactorGraph.h" +#include "BayesNet.h" +#include "BayesTree.h" + +namespace gtsam { + + template + class ISAM: public BayesTree { + + public: + + /** Create an empty Bayes Tree */ + ISAM(); + + /** Create a Bayes Tree from a Bayes Net */ + ISAM(const BayesNet& bayesNet); + + /** Destructor */ + virtual ~ISAM() { + } + + typedef typename BayesTree::sharedClique sharedClique; + + typedef typename BayesTree::Cliques Cliques; + + /** + * iSAM. (update_internal provides access to list of orphans for drawing purposes) + */ + template + void update_internal(const FactorGraph& newFactors, Cliques& orphans); + template + void update(const FactorGraph& newFactors); + + }; // ISAM + +} /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index c5a30140c..151bc37cc 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -75,19 +75,23 @@ testSymbolicBayesNet_LDADD = libgtsam.la headers += inference.h inference-inl.h headers += FactorGraph.h FactorGraph-inl.h headers += BayesNet.h BayesNet-inl.h -headers += BayesTree.h BayesTree-inl.h GaussianBayesTree.h -sources += GaussianBayesTree.cpp -check_PROGRAMS += testFactorgraph testBayesTree testGaussianBayesTree testInference testOrdering +headers += BayesTree.h BayesTree-inl.h +headers += ISAM.h ISAM-inl.h GaussianISAM.h +sources += GaussianISAM.cpp +check_PROGRAMS += testFactorgraph testInference testOrdering +check_PROGRAMS += testBayesTree testISAM testGaussianISAM testFactorgraph_SOURCES = testFactorgraph.cpp -testBayesTree_SOURCES = $(example) testBayesTree.cpp -testGaussianBayesTree_SOURCES = $(example) testGaussianBayesTree.cpp testInference_SOURCES = $(example) testInference.cpp testFactorgraph_LDADD = libgtsam.la -testBayesTree_LDADD = libgtsam.la -testGaussianBayesTree_LDADD = libgtsam.la testInference_LDADD = libgtsam.la testOrdering_SOURCES = testOrdering.cpp testOrdering_LDADD = libgtsam.la +testBayesTree_SOURCES = $(example) testBayesTree.cpp +testBayesTree_LDADD = libgtsam.la +testGaussianISAM_SOURCES = $(example) testGaussianISAM.cpp +testGaussianISAM_LDADD = libgtsam.la +testISAM_SOURCES = $(example) testISAM.cpp +testISAM_LDADD = libgtsam.la # Binary Inference headers += BinaryConditional.h diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 81665c7fe..515258831 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -20,10 +20,6 @@ using namespace boost::assign; using namespace gtsam; typedef BayesTree SymbolicBayesTree; -typedef ISAM SymbolicISAM; - -//template class BayesTree; // todo: needed? -//template class ISAM; /* ************************************************************************* */ // SLAM example from RSS sqrtSAM paper @@ -34,9 +30,9 @@ SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), l2(new SymbolicConditional("l2","x1","x3")); // Bayes Tree for sqrtSAM example -SymbolicISAM createSlamSymbolicBayesTree(){ +SymbolicBayesTree createSlamSymbolicBayesTree(){ // Create using insert - SymbolicISAM bayesTree_slam; + SymbolicBayesTree bayesTree_slam; bayesTree_slam.insert(x3); bayesTree_slam.insert(x2); bayesTree_slam.insert(x1); @@ -58,8 +54,8 @@ SymbolicConditional::shared_ptr X(new SymbolicConditional("X", "E")); // Bayes Tree for Asia example -SymbolicISAM createAsiaSymbolicBayesTree() { - SymbolicISAM bayesTree; +SymbolicBayesTree createAsiaSymbolicBayesTree() { + SymbolicBayesTree bayesTree; bayesTree.insert(B); bayesTree.insert(L); bayesTree.insert(E); @@ -128,7 +124,7 @@ TEST( BayesTree, removePath ) D(new SymbolicConditional("D", "C")), E(new SymbolicConditional("E", "B")), F(new SymbolicConditional("F", "E")); - SymbolicISAM bayesTree; + SymbolicBayesTree bayesTree; bayesTree.insert(A); bayesTree.insert(B); bayesTree.insert(C); @@ -142,11 +138,11 @@ TEST( BayesTree, removePath ) expected.push_factor("A","B"); expected.push_factor("A"); expected.push_factor("A","C"); - SymbolicISAM::Cliques expectedOrphans; + SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["D"], bayesTree["E"]; FactorGraph factors; - SymbolicISAM::Cliques orphans; + SymbolicBayesTree::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["C"]); CHECK(assert_equal((FactorGraph)expected, factors)); CHECK(assert_equal(expectedOrphans, orphans)); @@ -154,7 +150,7 @@ TEST( BayesTree, removePath ) // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected2; expected2.push_factor("B","E"); - SymbolicISAM::Cliques expectedOrphans2; + SymbolicBayesTree::Cliques expectedOrphans2; expectedOrphans2 += bayesTree["F"]; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["E"]); @@ -165,11 +161,11 @@ TEST( BayesTree, removePath ) /* ************************************************************************* */ TEST( BayesTree, removePath2 ) { - SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); + SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique B FactorGraph factors; - SymbolicISAM::Cliques orphans; + SymbolicBayesTree::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["B"]); // Check expected outcome @@ -178,7 +174,7 @@ TEST( BayesTree, removePath2 ) expected.push_factor("B","L"); expected.push_factor("B"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicISAM::Cliques expectedOrphans; + SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } @@ -186,11 +182,11 @@ TEST( BayesTree, removePath2 ) /* ************************************************************************* */ TEST( BayesTree, removePath3 ) { - SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); + SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique S FactorGraph factors; - SymbolicISAM::Cliques orphans; + SymbolicBayesTree::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["S"]); // Check expected outcome @@ -200,22 +196,22 @@ TEST( BayesTree, removePath3 ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicISAM::Cliques expectedOrphans; + SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } /* ************************************************************************* */ -TEST( ISAM, removeTop ) +TEST( BayesTree, removeTop ) { - SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); + SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // create a new factor to be inserted boost::shared_ptr newFactor(new SymbolicFactor("B","S")); // Remove the contaminated part of the Bayes tree FactorGraph factors; - SymbolicISAM::Cliques orphans; + SymbolicBayesTree::Cliques orphans; bayesTree.removeTop(newFactor, factors, orphans); // Check expected outcome @@ -225,26 +221,26 @@ TEST( ISAM, removeTop ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicISAM::Cliques expectedOrphans; + SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); // Try removeTop again with a factor that should not change a thing boost::shared_ptr newFactor2(new SymbolicFactor("B")); FactorGraph factors2; - SymbolicISAM::Cliques orphans2; + SymbolicBayesTree::Cliques orphans2; bayesTree.removeTop(newFactor2, factors2, orphans2); SymbolicFactorGraph expected2; CHECK(assert_equal((FactorGraph)expected2, factors2)); - SymbolicISAM::Cliques expectedOrphans2; + SymbolicBayesTree::Cliques expectedOrphans2; CHECK(assert_equal(expectedOrphans2, orphans2)); } /* ************************************************************************* */ -TEST( ISAM, removeTop2 ) +TEST( BayesTree, removeTop2 ) { - SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); + SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // create two factors to be inserted SymbolicFactorGraph newFactors; @@ -253,7 +249,7 @@ TEST( ISAM, removeTop2 ) // Remove the contaminated part of the Bayes tree FactorGraph factors; - SymbolicISAM::Cliques orphans; + SymbolicBayesTree::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removeTop(newFactors); // Check expected outcome @@ -263,77 +259,11 @@ TEST( ISAM, removeTop2 ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicISAM::Cliques expectedOrphans; + SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } -/* ************************************************************************* */ -TEST( ISAM, iSAM ) -{ - SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); - - // 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 - SymbolicISAM 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)); -} - -/* ************************************************************************* */ -TEST( ISAM, iSAM_slam ) -{ - // Create using insert - SymbolicISAM bayesTree_slam = createSlamSymbolicBayesTree(); - - //New conditionals for the expected Bayes tree - SymbolicConditional::shared_ptr - l1_(new SymbolicConditional("l1","x1","x2","x3")); - - // Create expected Bayes tree - SymbolicISAM expected_slam; - expected_slam.insert(x3); - expected_slam.insert(x2); - expected_slam.insert(x1); - expected_slam.insert(l1_); - expected_slam.insert(l2); - - - // create new factors to be inserted - SymbolicFactorGraph factorGraph_slam; - factorGraph_slam.push_factor("x3","l1"); - factorGraph_slam.push_factor("x3"); - - // do incremental inference - bayesTree_slam.update(factorGraph_slam); - - // Check whether the same - CHECK(assert_equal(expected_slam,bayesTree_slam)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testGaussianBayesTree.cpp b/cpp/testGaussianISAM.cpp similarity index 89% rename from cpp/testGaussianBayesTree.cpp rename to cpp/testGaussianISAM.cpp index d80380993..689dc7206 100644 --- a/cpp/testGaussianBayesTree.cpp +++ b/cpp/testGaussianISAM.cpp @@ -1,6 +1,6 @@ /** - * @file testGaussianBayesTree.cpp - * @brief Unit tests for GaussianBayesTree + * @file testGaussianISAM.cpp + * @brief Unit tests for GaussianISAM * @author Michael Kaess */ @@ -12,8 +12,8 @@ using namespace boost::assign; #include "Ordering.h" #include "GaussianBayesNet.h" -#include "BayesTree-inl.h" -#include "GaussianBayesTree.h" +#include "ISAM-inl.h" +#include "GaussianISAM.h" #include "smallExample.h" using namespace std; @@ -25,6 +25,62 @@ using namespace gtsam; double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; +/* ************************************************************************* */ +TEST( ISAM, iSAM_smoother ) +{ + // Create smoother with 7 nodes + GaussianFactorGraph smoother = createSmoother(7); + + // run iSAM for every factor + GaussianISAM actual; + BOOST_FOREACH(boost::shared_ptr factor, smoother) { + GaussianFactorGraph factorGraph; + factorGraph.push_back(factor); + actual.update(factorGraph); + } + + // Create expected Bayes Tree by solving smoother with "natural" ordering + Ordering ordering; + for (int t = 1; t <= 7; t++) ordering += symbol('x', t); + GaussianISAM expected(smoother.eliminate(ordering)); + + // Check whether BayesTree is correct + CHECK(assert_equal(expected, actual)); + + // obtain solution + VectorConfig e; // expected solution + Vector v = Vector_(2, 0., 0.); + for (int i=1; i<=7; i++) + e.insert(symbol('x', i), v); + VectorConfig optimized = optimize(actual); // actual solution + CHECK(assert_equal(e, optimized)); +} + +/* ************************************************************************* */ +TEST( ISAM, iSAM_smoother2 ) +{ + // Create smoother with 7 nodes + GaussianFactorGraph smoother = createSmoother(7); + + // Create initial tree from first 4 timestamps in reverse order ! + Ordering ord; ord += "x4","x3","x2","x1"; + GaussianFactorGraph factors1; + for (int i=0;i<7;i++) factors1.push_back(smoother[i]); + GaussianISAM actual(factors1.eliminate(ord)); + + // run iSAM with remaining factors + GaussianFactorGraph factors2; + for (int i=7;i<13;i++) factors2.push_back(smoother[i]); + actual.update(factors2); + + // Create expected Bayes Tree by solving smoother with "natural" ordering + Ordering ordering; + for (int t = 1; t <= 7; t++) ordering += symbol('x', t); + GaussianISAM expected(smoother.eliminate(ordering)); + + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* * Bayes tree for smoother with "natural" ordering: C1 x6 x7 @@ -46,17 +102,17 @@ TEST( BayesTree, linear_smoother_shortcuts ) GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); // Create the Bayes tree - GaussianBayesTree bayesTree(chordalBayesNet); + GaussianISAM bayesTree(chordalBayesNet); LONGS_EQUAL(6,bayesTree.size()); // Check the conditional P(Root|Root) GaussianBayesNet empty; - GaussianBayesTree::sharedClique R = bayesTree.root(); + GaussianISAM::sharedClique R = bayesTree.root(); GaussianBayesNet actual1 = R->shortcut(R); CHECK(assert_equal(empty,actual1,1e-4)); // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree["x5"]; + GaussianISAM::sharedClique C2 = bayesTree["x5"]; GaussianBayesNet actual2 = C2->shortcut(R); CHECK(assert_equal(empty,actual2,1e-4)); @@ -65,7 +121,7 @@ TEST( BayesTree, linear_smoother_shortcuts ) Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3); - GaussianBayesTree::sharedClique C3 = bayesTree["x4"]; + GaussianISAM::sharedClique C3 = bayesTree["x4"]; GaussianBayesNet actual3 = C3->shortcut(R); CHECK(assert_equal(expected3,actual3,1e-4)); @@ -74,7 +130,7 @@ TEST( BayesTree, linear_smoother_shortcuts ) Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4); - GaussianBayesTree::sharedClique C4 = bayesTree["x3"]; + GaussianISAM::sharedClique C4 = bayesTree["x3"]; GaussianBayesNet actual4 = C4->shortcut(R); CHECK(assert_equal(expected4,actual4,1e-4)); } @@ -115,7 +171,7 @@ TEST( BayesTree, balanced_smoother_marginals ) CHECK(assert_equal(expectedSolution,actualSolution,1e-4)); // Create the Bayes tree - GaussianBayesTree bayesTree(chordalBayesNet); + GaussianISAM bayesTree(chordalBayesNet); LONGS_EQUAL(4,bayesTree.size()); // Check marginal on x1 @@ -154,23 +210,23 @@ TEST( BayesTree, balanced_smoother_shortcuts ) // Create the Bayes tree GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); - GaussianBayesTree bayesTree(chordalBayesNet); + GaussianISAM bayesTree(chordalBayesNet); // Check the conditional P(Root|Root) GaussianBayesNet empty; - GaussianBayesTree::sharedClique R = bayesTree.root(); + GaussianISAM::sharedClique R = bayesTree.root(); GaussianBayesNet actual1 = R->shortcut(R); CHECK(assert_equal(empty,actual1,1e-4)); // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree["x3"]; + GaussianISAM::sharedClique C2 = bayesTree["x3"]; GaussianBayesNet actual2 = C2->shortcut(R); CHECK(assert_equal(empty,actual2,1e-4)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; GaussianBayesNet expected3; expected3.push_back(p_x2_x4); - GaussianBayesTree::sharedClique C3 = bayesTree["x1"]; + GaussianISAM::sharedClique C3 = bayesTree["x1"]; GaussianBayesNet actual3 = C3->shortcut(R); CHECK(assert_equal(expected3,actual3,1e-4)); } @@ -185,14 +241,14 @@ TEST( BayesTree, balanced_smoother_clique_marginals ) // Create the Bayes tree GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); - GaussianBayesTree bayesTree(chordalBayesNet); + GaussianISAM bayesTree(chordalBayesNet); // Check the clique marginal P(C3) GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2); Vector sigma = repeat(2, 0.707107); Matrix A12 = (-0.5)*eye(2); push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma); - GaussianBayesTree::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; + GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; FactorGraph marginal = C3->marginal(R); GaussianBayesNet actual = eliminate(marginal,C3->keys()); CHECK(assert_equal(expected,actual,1e-4)); @@ -208,7 +264,7 @@ TEST( BayesTree, balanced_smoother_joint ) // Create the Bayes tree GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); - GaussianBayesTree bayesTree(chordalBayesNet); + GaussianISAM bayesTree(chordalBayesNet); // Conditional density elements reused by both tests Vector sigma = repeat(2, 0.786146); @@ -243,62 +299,6 @@ TEST( BayesTree, balanced_smoother_joint ) CHECK(assert_equal(expected4,actual4,1e-4)); } -/* ************************************************************************* * -TEST( BayesTree, iSAM_smoother ) -{ - // Create smoother with 7 nodes - GaussianFactorGraph smoother = createSmoother(7); - - // run iSAM for every factor - GaussianBayesTree actual; - BOOST_FOREACH(boost::shared_ptr factor, smoother) { - GaussianFactorGraph factorGraph; - factorGraph.push_back(factor); - actual.update(factorGraph); - } - - // Create expected Bayes Tree by solving smoother with "natural" ordering - Ordering ordering; - for (int t = 1; t <= 7; t++) ordering += symbol('x', t); - GaussianBayesTree expected(smoother.eliminate(ordering)); - - // Check whether BayesTree is correct - CHECK(assert_equal(expected, actual)); - - // obtain solution - VectorConfig e; // expected solution - Vector v = Vector_(2, 0., 0.); - for (int i=1; i<=7; i++) - e.insert(symbol('x', i), v); - VectorConfig optimized = optimize(actual); // actual solution - CHECK(assert_equal(e, optimized)); -} - -/* ************************************************************************* * -TEST( BayesTree, iSAM_smoother2 ) -{ - // Create smoother with 7 nodes - GaussianFactorGraph smoother = createSmoother(7); - - // Create initial tree from first 4 timestamps in reverse order ! - Ordering ord; ord += "x4","x3","x2","x1"; - GaussianFactorGraph factors1; - for (int i=0;i<7;i++) factors1.push_back(smoother[i]); - GaussianBayesTree actual(factors1.eliminate(ord)); - - // run iSAM with remaining factors - GaussianFactorGraph factors2; - for (int i=7;i<13;i++) factors2.push_back(smoother[i]); - actual.update(factors2); - - // Create expected Bayes Tree by solving smoother with "natural" ordering - Ordering ordering; - for (int t = 1; t <= 7; t++) ordering += symbol('x', t); - GaussianBayesTree expected(smoother.eliminate(ordering)); - - CHECK(assert_equal(expected, actual)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testISAM.cpp b/cpp/testISAM.cpp new file mode 100644 index 000000000..93410abf1 --- /dev/null +++ b/cpp/testISAM.cpp @@ -0,0 +1,143 @@ +/** + * @file testISAM.cpp + * @brief Unit tests for ISAM + * @author Michael Kaess + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include + +#include "SymbolicBayesNet.h" +#include "SymbolicFactorGraph.h" +#include "Ordering.h" +#include "GaussianBayesNet.h" +#include "ISAM-inl.h" +#include "smallExample.h" + +using namespace std; +using namespace gtsam; + +typedef ISAM SymbolicISAM; + +/* ************************************************************************* */ +// Some numbers that should be consistent among all smoother tests + +double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = + 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; + +/* ************************************************************************* */ + +// SLAM example from RSS sqrtSAM paper +SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), + x2(new SymbolicConditional("x2","x3")), + x1(new SymbolicConditional("x1","x2","x3")), + l1(new SymbolicConditional("l1","x1","x2")), + l2(new SymbolicConditional("l2","x1","x3")); + +// ISAM for sqrtSAM example +SymbolicISAM createSlamSymbolicISAM(){ + // Create using insert + SymbolicISAM bayesTree_slam; + bayesTree_slam.insert(x3); + bayesTree_slam.insert(x2); + bayesTree_slam.insert(x1); + bayesTree_slam.insert(l2); + bayesTree_slam.insert(l1); + return bayesTree_slam; +} + +/* ************************************************************************* */ + +// 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")); + +// ISAM for Asia example +SymbolicISAM createAsiaSymbolicISAM() { + SymbolicISAM bayesTree; + bayesTree.insert(B); + bayesTree.insert(L); + bayesTree.insert(E); + bayesTree.insert(S); + bayesTree.insert(T); + bayesTree.insert(X); + return bayesTree; +} + +/* ************************************************************************* */ +TEST( ISAM, iSAM ) +{ + SymbolicISAM bayesTree = createAsiaSymbolicISAM(); + + // 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 + SymbolicISAM 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)); +} + +/* ************************************************************************* */ +TEST( ISAM, iSAM_slam ) +{ + // Create using insert + SymbolicISAM bayesTree_slam = createSlamSymbolicISAM(); + + //New conditionals for the expected Bayes tree + SymbolicConditional::shared_ptr + l1_(new SymbolicConditional("l1","x1","x2","x3")); + + // Create expected Bayes tree + SymbolicISAM expected_slam; + expected_slam.insert(x3); + expected_slam.insert(x2); + expected_slam.insert(x1); + expected_slam.insert(l1_); + expected_slam.insert(l2); + + + // create new factors to be inserted + SymbolicFactorGraph factorGraph_slam; + factorGraph_slam.push_factor("x3","l1"); + factorGraph_slam.push_factor("x3"); + + // do incremental inference + bayesTree_slam.update(factorGraph_slam); + + // Check whether the same + CHECK(assert_equal(expected_slam,bayesTree_slam)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */