From dd3795ad5a1b41ceba99d456afad4d950dd6fb80 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 30 Oct 2009 03:48:32 +0000 Subject: [PATCH] Symbolic eliminate now works, new compilation unit SymbolicFactor --- cpp/BayesChain-inl.h | 12 ++++- cpp/BayesChain.h | 3 ++ cpp/Makefile.am | 19 +++++--- cpp/SymbolicBayesChain.h | 8 ++++ cpp/SymbolicConditional.h | 8 +++- cpp/SymbolicFactor.cpp | 69 +++++++++++++++++++++++++++++ cpp/SymbolicFactor.h | 78 +++++++++++++++++++++++++++++++++ cpp/SymbolicFactorGraph.cpp | 54 +++++------------------ cpp/SymbolicFactorGraph.h | 69 ++++++----------------------- cpp/testSymbolicFactor.cpp | 19 ++++++++ cpp/testSymbolicFactorGraph.cpp | 32 +++++++++++++- 11 files changed, 261 insertions(+), 110 deletions(-) create mode 100644 cpp/SymbolicFactor.cpp create mode 100644 cpp/SymbolicFactor.h create mode 100644 cpp/testSymbolicFactor.cpp diff --git a/cpp/BayesChain-inl.h b/cpp/BayesChain-inl.h index 17bda1a49..8a51a29db 100644 --- a/cpp/BayesChain-inl.h +++ b/cpp/BayesChain-inl.h @@ -20,10 +20,10 @@ namespace gtsam { /* ************************************************************************* */ template void BayesChain::print(const string& s) const { - cout << s << ":" << endl; + cout << s << ":\n"; BOOST_FOREACH(string key, keys_) { const_iterator it = nodes_.find(key); - it->second->print("\nNode[" + key + "]"); + it->second->print("Node[" + key + "]"); } } @@ -42,6 +42,14 @@ namespace gtsam { return true; } + /* ************************************************************************* */ + template + void BayesChain::insert + (const string& key, boost::shared_ptr node) { + keys_.push_front(key); + nodes_.insert(make_pair(key,node)); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/BayesChain.h b/cpp/BayesChain.h index 70072a820..e048f4776 100644 --- a/cpp/BayesChain.h +++ b/cpp/BayesChain.h @@ -42,6 +42,9 @@ namespace gtsam { /** check equality */ bool equals(const BayesChain& other, double tol = 1e-9) const; + + /** insert: use reverse topological sort (i.e. parents last) */ + void insert(const std::string& key, boost::shared_ptr node); }; } /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index da71d4176..6185f6404 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -57,23 +57,28 @@ testVector_LDADD = libgtsam.la testMatrix_LDADD = libgtsam.la # nodes -sources += VectorConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp +sources += VectorConfig.cpp Ordering.cpp +sources += SymbolicFactor.cpp LinearFactor.cpp NonlinearFactor.cpp sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp -check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian +check_PROGRAMS += testVectorConfig testSymbolicFactor testLinearFactor +check_PROGRAMS += testConditionalGaussian testNonlinearFactor +check_PROGRAMS += testLinearConstraint testConstrainedConditionalGaussian example = smallExample.cpp -testVectorConfig_SOURCES = testVectorConfig.cpp +testVectorConfig_SOURCES = testVectorConfig.cpp +testSymbolicFactor_SOURCES = $(example) testSymbolicFactor.cpp testLinearFactor_SOURCES = $(example) testLinearFactor.cpp testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp -testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp -testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp +testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp +testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp -testVectorConfig_LDADD = libgtsam.la +testVectorConfig_LDADD = libgtsam.la +testSymbolicFactor_LDADD = libgtsam.la testLinearFactor_LDADD = libgtsam.la testConditionalGaussian_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la testLinearConstraint_LDADD = libgtsam.la -testConstrainedConditionalGaussian_LDADD = libgtsam.la +testConstrainedConditionalGaussian_LDADD = libgtsam.la # not the correct way, I'm sure: Kai ? timeLinearFactor: timeLinearFactor.cpp diff --git a/cpp/SymbolicBayesChain.h b/cpp/SymbolicBayesChain.h index 392710284..a8bf8df66 100644 --- a/cpp/SymbolicBayesChain.h +++ b/cpp/SymbolicBayesChain.h @@ -27,6 +27,14 @@ namespace gtsam { class SymbolicBayesChain: public BayesChain { public: + /** convenience typename for a shared pointer to this class */ + typedef boost::shared_ptr shared_ptr; + + /** + * Empty constructor + */ + SymbolicBayesChain() {} + /** * Construct from a map of nodes */ diff --git a/cpp/SymbolicConditional.h b/cpp/SymbolicConditional.h index 05653f030..5088781d2 100644 --- a/cpp/SymbolicConditional.h +++ b/cpp/SymbolicConditional.h @@ -8,8 +8,11 @@ #pragma once -#include "Testable.h" +#include +#include +#include #include // TODO: make cpp file +#include "Testable.h" namespace gtsam { @@ -23,6 +26,7 @@ namespace gtsam { public: + /** convenience typename for a shared pointer to this class */ typedef boost::shared_ptr shared_ptr; /** @@ -54,7 +58,7 @@ namespace gtsam { /** print */ void print(const std::string& s = "SymbolicConditional") const { - std::cout << s << std::endl; + std::cout << s; BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent; std::cout << std::endl; } diff --git a/cpp/SymbolicFactor.cpp b/cpp/SymbolicFactor.cpp new file mode 100644 index 000000000..8c9fc0a2a --- /dev/null +++ b/cpp/SymbolicFactor.cpp @@ -0,0 +1,69 @@ +/* + * SymbolicFactor.cpp + * + * Created on: Oct 29, 2009 + * Author: Frank Dellaert + */ + +#include +#include +#include +#include +#include "SymbolicConditional.h" +#include "SymbolicFactor.h" + +using namespace std; + +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +namespace gtsam { + + /* ************************************************************************* */ + SymbolicFactor::SymbolicFactor(const vector & factors) { + + // store keys in a map to make them unique (set is not portable) + map map; + BOOST_FOREACH(shared_ptr factor, factors) + BOOST_FOREACH(string key, factor->keys()) + map.insert(make_pair(key,key)); + + // create the unique keys + string key,val; + FOREACH_PAIR(key, val, map) + keys_.push_back(key); + } + + /* ************************************************************************* */ + void SymbolicFactor::print(const string& s) const { + cout << s << " "; + BOOST_FOREACH(string key, keys_) cout << " " << key; + cout << endl; + } + + /* ************************************************************************* */ + bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const { + return keys_ == other.keys_; + } + + /* ************************************************************************* */ + pair + SymbolicFactor::eliminate(const string& key) const + { + // get keys from input factor + list separator; + BOOST_FOREACH(string j,keys_) + if (j!=key) separator.push_back(j); + + // start empty remaining factor to be returned + boost::shared_ptr lf(new SymbolicFactor(separator)); + + // create SymbolicConditional on separator + SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator)); + + return make_pair(cg,lf); + } + + /* ************************************************************************* */ + +} diff --git a/cpp/SymbolicFactor.h b/cpp/SymbolicFactor.h new file mode 100644 index 000000000..8d4b5cf44 --- /dev/null +++ b/cpp/SymbolicFactor.h @@ -0,0 +1,78 @@ +/* + * SymbolicFactor.h + * + * Created on: Oct 29, 2009 + * Author: Frank Dellaert + */ + +#ifndef SYMBOLICFACTOR_H_ +#define SYMBOLICFACTOR_H_ + +#include +#include +#include +#include +#include "Testable.h" + +namespace gtsam { + + class SymbolicConditional; + + /** Symbolic Factor */ + class SymbolicFactor: public Testable { + private: + + std::list keys_; + + public: + + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor from a list of keys + */ + SymbolicFactor(std::list keys) : + keys_(keys) { + } + + /** + * Constructor that combines a set of factors + * @param factors Set of factors to combine + */ + SymbolicFactor(const std::vector & factors); + + /** print */ + void print(const std::string& s = "SymbolicFactor") const; + + /** check equality */ + bool equals(const SymbolicFactor& other, double tol = 1e-9) const; + + /** + * Find all variables + * @return The set of all variable keys + */ + std::list keys() const { + return keys_; + } + + /** + * eliminate one of the variables connected to this factor + * @param key the key of the node to be eliminated + * @return a new factor and a symbolic conditional on the eliminated variable + */ + std::pair, shared_ptr> + eliminate(const std::string& key) const; + + /** + * Check if empty factor + */ + inline bool empty() const { + return keys_.empty(); + } + + + }; + +} + +#endif /* SYMBOLICFACTOR_H_ */ diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index db079ff38..d11972a0b 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -6,59 +6,27 @@ */ #include -#include +#include "Ordering.h" #include "SymbolicFactorGraph.h" +#include "BayesChain-inl.h" +#include "SymbolicBayesChain.h" using namespace std; -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { /* ************************************************************************* */ - SymbolicFactor::SymbolicFactor(const vector & factors) { - - // store keys in a map to make them unique (set is not portable) - map map; - BOOST_FOREACH(shared_ptr factor, factors) - BOOST_FOREACH(string key, factor->keys()) - map.insert(make_pair(key,key)); - - // create the unique keys - string key,val; - FOREACH_PAIR(key, val, map) - keys_.push_back(key); - } - - /* ************************************************************************* */ - void SymbolicFactor::print(const string& s) const { - cout << s << " "; - BOOST_FOREACH(string key, keys_) cout << " " << key; - cout << endl; - } - - /* ************************************************************************* */ - bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const { - return keys_ == other.keys_; - } - - /* ************************************************************************* */ - pair - SymbolicFactor::eliminate(const string& key) const + SymbolicBayesChain::shared_ptr + SymbolicFactorGraph::eliminate(const Ordering& ordering) { - // get keys from input factor - list separator; - BOOST_FOREACH(string j,keys_) - if (j!=key) separator.push_back(j); + SymbolicBayesChain::shared_ptr bayesChain (new SymbolicBayesChain()); - // start empty remaining factor to be returned - boost::shared_ptr lf(new SymbolicFactor(separator)); + BOOST_FOREACH(string key, ordering) { + SymbolicConditional::shared_ptr conditional = eliminateOne(key); + bayesChain->insert(key,conditional); + } - // create SymbolicConditional on separator - SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator)); - - return make_pair(cg,lf); + return bayesChain; } /* ************************************************************************* */ diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h index cdf6915fe..650eb301d 100644 --- a/cpp/SymbolicFactorGraph.h +++ b/cpp/SymbolicFactorGraph.h @@ -11,72 +11,25 @@ #include #include #include "FactorGraph.h" -#include "SymbolicConditional.h" +#include "SymbolicFactor.h" namespace gtsam { - /** Symbolic Factor */ - class SymbolicFactor: public Testable { - private: - - std::list keys_; - - public: - - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor from a list of keys - */ - SymbolicFactor(std::list keys) : - keys_(keys) { - } - - /** - * Constructor that combines a set of factors - * @param factors Set of factors to combine - */ - SymbolicFactor(const std::vector & factors); - - /** print */ - void print(const std::string& s = "SymbolicFactor") const; - - /** check equality */ - bool equals(const SymbolicFactor& other, double tol = 1e-9) const; - - /** - * Find all variables - * @return The set of all variable keys - */ - std::list keys() const { - return keys_; - } - - /** - * eliminate one of the variables connected to this factor - * @param key the key of the node to be eliminated - * @return a new factor and a symbolic conditional on the eliminated variable - */ - std::pair - eliminate(const std::string& key) const; - - /** - * Check if empty factor - */ - inline bool empty() const { - return keys_.empty(); - } - - - }; + class SymbolicBayesChain; /** Symbolic Factor Graph */ class SymbolicFactorGraph: public FactorGraph { public: + /** + * Construct empty factor graph + */ SymbolicFactorGraph() { } + /** + * Construct from a factor graph of any type + */ template SymbolicFactorGraph(const FactorGraph& fg) { for (size_t i = 0; i < fg.size(); i++) { @@ -87,6 +40,12 @@ namespace gtsam { } } + /** + * eliminate factor graph in place(!) in the given order, yielding + * a chordal Bayes net + */ + boost::shared_ptr eliminate(const Ordering& ordering); + }; } diff --git a/cpp/testSymbolicFactor.cpp b/cpp/testSymbolicFactor.cpp new file mode 100644 index 000000000..ad7636ed4 --- /dev/null +++ b/cpp/testSymbolicFactor.cpp @@ -0,0 +1,19 @@ +/** + * @file testSymbolicFactor.cpp + * @brief Unit tests for a symbolic Factor + * @author Frank Dellaert + */ + +#include + +#include "SymbolicFactor.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 9f4717971..761d91b98 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -9,6 +9,9 @@ #include "smallExample.h" #include "FactorGraph-inl.h" #include "SymbolicFactorGraph.h" +#include "BayesChain-inl.h" +#include "SymbolicConditional.h" +#include "SymbolicBayesChain.h" using namespace std; using namespace gtsam; @@ -100,7 +103,7 @@ TEST( LinearFactorGraph, removeAndCombineFactors ) } /* ************************************************************************* */ -TEST( LinearFactorGraph, eliminateOne_x1 ) +TEST( LinearFactorGraph, eliminateOne ) { // create a test graph LinearFactorGraph factorGraph = createLinearFactorGraph(); @@ -116,6 +119,33 @@ TEST( LinearFactorGraph, eliminateOne_x1 ) CHECK(assert_equal(expected,*actual)); } +/* ************************************************************************* */ +TEST( LinearFactorGraph, eliminate ) +{ + // create expected Chordal bayes Net + SymbolicConditional::shared_ptr c1(new SymbolicConditional()); + SymbolicConditional::shared_ptr c2(new SymbolicConditional("x1")); + SymbolicConditional::shared_ptr c3(new SymbolicConditional("l1", "x1")); + + SymbolicBayesChain expected; + expected.insert("x1", c1); + expected.insert("l1", c2); + expected.insert("x2", c3); + + // create a test graph + LinearFactorGraph factorGraph = createLinearFactorGraph(); + SymbolicFactorGraph fg(factorGraph); + + // eliminate it + Ordering ordering; + ordering.push_back("x2"); + ordering.push_back("l1"); + ordering.push_back("x1"); + SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering); + + CHECK(assert_equal(expected,*actual)); +} + /* ************************************************************************* */ int main() { TestResult tr;