From 1f792a53ea0485a024555f2de9f7935c40d0f82e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Oct 2009 02:57:38 +0000 Subject: [PATCH] Made some progress on symbolic analysis --- cpp/BayesChain-inl.h | 47 ++++++++++++++++++++++++++++++ cpp/BayesChain.h | 23 +++++++++++++-- cpp/Makefile.am | 18 +++++++----- cpp/SymbolicBayesChain-inl.h | 17 +++++++---- cpp/SymbolicBayesChain.cpp | 43 ++++++++++----------------- cpp/SymbolicBayesChain.h | 25 +++++++--------- cpp/SymbolicConditional.h | 53 ++++++++++++++++++++++++++++++++++ cpp/testBayesTree.cpp | 7 ++++- cpp/testSymbolicBayesChain.cpp | 20 ++++++++++++- 9 files changed, 195 insertions(+), 58 deletions(-) create mode 100644 cpp/BayesChain-inl.h create mode 100644 cpp/SymbolicConditional.h diff --git a/cpp/BayesChain-inl.h b/cpp/BayesChain-inl.h new file mode 100644 index 000000000..17bda1a49 --- /dev/null +++ b/cpp/BayesChain-inl.h @@ -0,0 +1,47 @@ +/** + * @file BayesChain-inl.h + * @brief Bayes chain template definitions + * @author Frank Dellaert + */ + +#include +#include +#include + +#include "BayesChain.h" + +using namespace std; + +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesChain::print(const string& s) const { + cout << s << ":" << endl; + BOOST_FOREACH(string key, keys_) { + const_iterator it = nodes_.find(key); + it->second->print("\nNode[" + key + "]"); + } + } + + /* ************************************************************************* */ + template + bool BayesChain::equals(const BayesChain& cbn, double tol) const { + const_iterator it1 = nodes_.begin(), it2 = cbn.nodes_.begin(); + if(nodes_.size() != cbn.nodes_.size()) return false; + for(; it1 != nodes_.end(); it1++, it2++) { + const string& j1 = it1->first, j2 = it2->first; + boost::shared_ptr node1 = it1->second, node2 = it2->second; + if (j1 != j2) return false; + if (!node1->equals(*node2,tol)) + return false; + } + return true; + } + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/cpp/BayesChain.h b/cpp/BayesChain.h index 8d90e9726..70072a820 100644 --- a/cpp/BayesChain.h +++ b/cpp/BayesChain.h @@ -8,10 +8,12 @@ #pragma once -#include +#include #include #include +#include "Testable.h" + namespace gtsam { /** @@ -21,8 +23,25 @@ namespace gtsam { * a ConditionalProbabilityTable, a ConditionalGaussian, or a SymbolicConditional. */ template - class BayesChain { + class BayesChain: public Testable > { + protected: + + /** nodes keys stored in topological sort order, i.e. from parents to children */ + std::list keys_; + + /** nodes stored on key */ + typedef typename std::map > Nodes; + Nodes nodes_; + + typedef typename Nodes::const_iterator const_iterator; + public: + + /** print */ + void print(const std::string& s = "") const; + + /** check equality */ + bool equals(const BayesChain& other, double tol = 1e-9) const; }; } /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e0d65aa38..7e510494f 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -80,17 +80,14 @@ timeLinearFactor: timeLinearFactor.cpp timeLinearFactor: CXXFLAGS += -I /opt/local/include timeLinearFactor: LDFLAGS += -L.libs -lgtsam -# not the correct way, I'm sure: Kai ? -timeLinearFactorGraph: timeLinearFactorGraph.cpp -timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I .. -timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite - # graphs sources += LinearFactorGraph.cpp -sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp +#sources += BayesChain.cpp SymbolicBayesChain.cpp +sources += ChordalBayesNet.cpp sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp -check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer -check_PROGRAMS += testSymbolicBayesChain testChordalBayesNet testBayesTree +check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph +check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer +#check_PROGRAMS += testSymbolicBayesChain testBayesTree check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph testFactorgraph_SOURCES = testFactorgraph.cpp testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp @@ -112,6 +109,11 @@ testBayesTree_LDADD = libgtsam.la testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la testConstrainedLinearFactorGraph_LDADD = libgtsam.la +# not the correct way, I'm sure: Kai ? +timeLinearFactorGraph: timeLinearFactorGraph.cpp +timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I .. +timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite + # geometry sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 diff --git a/cpp/SymbolicBayesChain-inl.h b/cpp/SymbolicBayesChain-inl.h index ebdb25b1d..05e693614 100644 --- a/cpp/SymbolicBayesChain-inl.h +++ b/cpp/SymbolicBayesChain-inl.h @@ -4,14 +4,21 @@ * @author Frank Dellaert */ +#include +#include + #include "SymbolicBayesChain.h" using namespace std; -using namespace gtsam; + +namespace gtsam { + + /* ************************************************************************* */ + template + SymbolicBayesChain::SymbolicBayesChain( + const FactorGraph& factorGraph, const Ordering& ordering) { + } /* ************************************************************************* */ -template -SymbolicBayesChain::SymbolicBayesChain(const FactorGraph& factorGraph) { -} -/* ************************************************************************* */ +} // namespace gtsam diff --git a/cpp/SymbolicBayesChain.cpp b/cpp/SymbolicBayesChain.cpp index 58e8625ed..0b7fcb5e1 100644 --- a/cpp/SymbolicBayesChain.cpp +++ b/cpp/SymbolicBayesChain.cpp @@ -4,40 +4,29 @@ * @author Frank Dellaert */ -#include #include #include +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + #include "SymbolicBayesChain.h" using namespace std; -using namespace gtsam; -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +namespace gtsam { + + typedef pair pp; + + /* ************************************************************************* */ + SymbolicBayesChain::SymbolicBayesChain(const std::map& nodes) { + BOOST_FOREACH(pp p, nodes) { + keys_.push_front(p.first); + nodes_.insert(p); + } + } /* ************************************************************************* */ -void SymbolicBayesChain::print(const string& s) const { - // BOOST_FOREACH(string key, keys) { - // const_iterator it = nodes.find(key); - // it->second->print("\nNode[" + key + "]"); - // } -} -/* ************************************************************************* */ -bool SymbolicBayesChain::equals(const SymbolicBayesChain& cbn, double tol) const { - // const_iterator it1 = nodes.begin(), it2 = cbn.nodes.begin(); - // - // if(nodes.size() != cbn.nodes.size()) return false; - // for(; it1 != nodes.end(); it1++, it2++){ - // const string& j1 = it1->first, j2 = it2->first; - // ConditionalGaussian::shared_ptr node1 = it1->second, node2 = it2->second; - // if (j1 != j2) return false; - // if (!node1->equals(*node2,tol)) - // return false; - // } - // return true; - return false; -} - -/* ************************************************************************* */ +} // namespace gtsam diff --git a/cpp/SymbolicBayesChain.h b/cpp/SymbolicBayesChain.h index 271be6d89..21d078dbd 100644 --- a/cpp/SymbolicBayesChain.h +++ b/cpp/SymbolicBayesChain.h @@ -15,37 +15,34 @@ #include "Testable.h" #include "BayesChain.h" #include "FactorGraph.h" +#include "SymbolicConditional.h" namespace gtsam { - /** - * Conditional node for use in a symbolic Bayes chain - */ - class SymbolicConditional { - }; + class Ordering; /** * Symbolic Bayes Chain, the (symbolic) result of eliminating a factor graph */ - class SymbolicBayesChain: public BayesChain , - public Testable { + class SymbolicBayesChain: public BayesChain { public: + /** + * Construct from a map of nodes + */ + SymbolicBayesChain(const std::map& nodes); + /** * Construct from any factor graph */ template - SymbolicBayesChain(const FactorGraph& factorGraph); + SymbolicBayesChain(const FactorGraph& factorGraph, + const Ordering& ordering); /** Destructor */ virtual ~SymbolicBayesChain() { } - - /** print */ - void print(const std::string& s = "") const; - - /** check equality */ - bool equals(const SymbolicBayesChain& other, double tol = 1e-9) const; }; } /// namespace gtsam diff --git a/cpp/SymbolicConditional.h b/cpp/SymbolicConditional.h new file mode 100644 index 000000000..64ba5120b --- /dev/null +++ b/cpp/SymbolicConditional.h @@ -0,0 +1,53 @@ +/** + * @file SymbolicConditional.h + * @brief Symbolic Conditional node for use in Bayes nets + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include "Testable.h" + +namespace gtsam { + + /** + * Conditional node for use in a Bayes nets + */ + class SymbolicConditional: Testable { + public: + + typedef boost::shared_ptr shared_ptr; + + /** + * No parents + */ + SymbolicConditional() { + } + + /** + * Single parent + */ + SymbolicConditional(const std::string& key) { + } + + /** + * Two parents + */ + SymbolicConditional(const std::string& key1, const std::string& key2) { + } + + /** print */ + void print(const std::string& s = "SymbolicConditional") const { + std::cout << s << std::endl; + } + + /** check equality */ + bool equals(const SymbolicConditional& other, double tol = 1e-9) const { + return false; + } + + }; + +} /// namespace gtsam diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index fa5223fd5..b43d95e6d 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -6,6 +6,7 @@ #include +#include "BayesChain-inl.h" #include "SymbolicBayesChain-inl.h" #include "smallExample.h" #include "BayesTree.h" @@ -16,7 +17,11 @@ using namespace gtsam; TEST( BayesTree, constructor ) { LinearFactorGraph factorGraph = createLinearFactorGraph(); - SymbolicBayesChain symbolicBayesChain(factorGraph); + Ordering ordering; + ordering.push_back("x2"); + ordering.push_back("l1"); + ordering.push_back("x1"); + SymbolicBayesChain symbolicBayesChain(factorGraph,ordering); BayesTree bayesTree(symbolicBayesChain); } diff --git a/cpp/testSymbolicBayesChain.cpp b/cpp/testSymbolicBayesChain.cpp index 705ba90b4..183eeac69 100644 --- a/cpp/testSymbolicBayesChain.cpp +++ b/cpp/testSymbolicBayesChain.cpp @@ -7,6 +7,7 @@ #include #include "smallExample.h" +#include "BayesChain-inl.h" #include "SymbolicBayesChain-inl.h" using namespace gtsam; @@ -14,8 +15,25 @@ using namespace gtsam; /* ************************************************************************* */ TEST( SymbolicBayesChain, constructor ) { + // Create manually + SymbolicConditional::shared_ptr x2(new SymbolicConditional("x1","l1")); + SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1")); + SymbolicConditional::shared_ptr x1(new SymbolicConditional()); + map nodes; + nodes.insert(make_pair("x2",x2)); + nodes.insert(make_pair("l1",l1)); + nodes.insert(make_pair("x1",x1)); + SymbolicBayesChain expected(nodes); + + // Create from a factor graph + Ordering ordering; + ordering.push_back("x2"); + ordering.push_back("l1"); + ordering.push_back("x1"); LinearFactorGraph factorGraph = createLinearFactorGraph(); - SymbolicBayesChain symbolicChordalBayesNet(factorGraph); + SymbolicBayesChain actual(factorGraph,ordering); + + //CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */