From 83e5286710a98b3f1a71a97426b9ab909249099e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 30 Oct 2009 04:54:11 +0000 Subject: [PATCH] ChordalBayesNet is now derived from BayesChain Explicit template instantiations --- cpp/BayesChain-inl.h | 12 +++++ cpp/BayesChain.h | 29 ++++++++++- cpp/ChordalBayesNet.cpp | 59 ++++----------------- cpp/ChordalBayesNet.h | 66 ++---------------------- cpp/LinearFactorGraph.cpp | 5 ++ cpp/Makefile.am | 5 +- cpp/SymbolicBayesChain.cpp | 5 +- cpp/SymbolicFactorGraph.cpp | 1 - cpp/smallExample.h | 9 +--- cpp/testBayesTree.cpp | 3 +- cpp/testChordalBayesNet.cpp | 2 - cpp/testConstrainedLinearFactorGraph.cpp | 4 +- cpp/testSymbolicBayesChain.cpp | 3 +- cpp/testSymbolicFactorGraph.cpp | 1 - 14 files changed, 70 insertions(+), 134 deletions(-) diff --git a/cpp/BayesChain-inl.h b/cpp/BayesChain-inl.h index 8a51a29db..81788c8e6 100644 --- a/cpp/BayesChain-inl.h +++ b/cpp/BayesChain-inl.h @@ -50,6 +50,18 @@ namespace gtsam { nodes_.insert(make_pair(key,node)); } + /* ************************************************************************* */ + template + void BayesChain::erase(const string& key) { + list::iterator it; + for (it=keys_.begin(); it != keys_.end(); ++it){ + if( strcmp(key.c_str(), (*it).c_str()) == 0 ) + break; + } + keys_.erase(it); + nodes_.erase(key); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/BayesChain.h b/cpp/BayesChain.h index e048f4776..bdedf6698 100644 --- a/cpp/BayesChain.h +++ b/cpp/BayesChain.h @@ -33,8 +33,6 @@ namespace gtsam { typedef typename std::map > Nodes; Nodes nodes_; - typedef typename Nodes::const_iterator const_iterator; - public: /** print */ @@ -43,8 +41,35 @@ namespace gtsam { /** check equality */ bool equals(const BayesChain& other, double tol = 1e-9) const; + /** size is the number of nodes */ + inline size_t size() const {return nodes_.size();} + /** insert: use reverse topological sort (i.e. parents last) */ void insert(const std::string& key, boost::shared_ptr node); + + /** delete */ + void erase(const std::string& key); + + inline boost::shared_ptr operator[](const std::string& key) const { + const_iterator cg = nodes_.find(key); // get node + assert( cg != nodes_.end() ); + return cg->second; + } + + /** return begin and end of the nodes. FD: breaks encapsulation? */ + typedef typename Nodes::const_iterator const_iterator; + const_iterator const begin() const {return nodes_.begin();} + const_iterator const end() const {return nodes_.end();} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(keys_); + ar & BOOST_SERIALIZATION_NVP(nodes_); + } }; } /// namespace gtsam diff --git a/cpp/ChordalBayesNet.cpp b/cpp/ChordalBayesNet.cpp index 750fdcfd9..c2855acf2 100644 --- a/cpp/ChordalBayesNet.cpp +++ b/cpp/ChordalBayesNet.cpp @@ -14,61 +14,22 @@ using namespace std; using namespace gtsam; +// Explicitly instantiate so we don't have to include everywhere +#include "BayesChain-inl.h" +template class BayesChain; + // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -/* ************************************************************************* */ -void ChordalBayesNet::print(const string& s) const { - BOOST_FOREACH(string key, keys) { - const_iterator it = nodes.find(key); - it->second->print("\nNode[" + key + "]"); - } -} - -/* ************************************************************************* */ -bool ChordalBayesNet::equals(const ChordalBayesNet& 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; -} - -/* ************************************************************************* */ -void ChordalBayesNet::insert(const string& key, ConditionalGaussian::shared_ptr node) -{ - keys.push_front(key); - nodes.insert(make_pair(key,node)); -} - -/* ************************************************************************* */ -void ChordalBayesNet::erase(const string& key) -{ - list::iterator it; - for (it=keys.begin(); it != keys.end(); ++it){ - if( strcmp(key.c_str(), (*it).c_str()) == 0 ) - break; - } - keys.erase(it); - nodes.erase(key); -} - /* ************************************************************************* */ boost::shared_ptr ChordalBayesNet::optimize() const { boost::shared_ptr result(new VectorConfig); /** solve each node in turn in topological sort order (parents first)*/ - BOOST_FOREACH(string key, keys) { - const_iterator cg = nodes.find(key); // get node - assert( cg != nodes.end() ); // make sure it exists + BOOST_FOREACH(string key, keys_) { + const_iterator cg = nodes_.find(key); // get node + assert( cg != nodes_.end() ); // make sure it exists Vector x = cg->second->solve(*result); // Solve for that variable result->insert(key,x); // store result in partial solution } @@ -81,9 +42,9 @@ pair ChordalBayesNet::matrix() const { // add the dimensions of all variables to get matrix dimension // and at the same time create a mapping from keys to indices size_t N=0; map indices; - BOOST_REVERSE_FOREACH(string key, keys) { + BOOST_REVERSE_FOREACH(string key, keys_) { // find corresponding node - const_iterator it = nodes.find(key); + const_iterator it = nodes_.find(key); indices.insert(make_pair(key,N)); N += it->second->dim(); } @@ -94,7 +55,7 @@ pair ChordalBayesNet::matrix() const { string key; size_t I; FOREACH_PAIR(key,I,indices) { // find corresponding node - const_iterator it = nodes.find(key); + const_iterator it = nodes_.find(key); ConditionalGaussian::shared_ptr cg = it->second; // get RHS and copy to d diff --git a/cpp/ChordalBayesNet.h b/cpp/ChordalBayesNet.h index 56380cabb..6cbe5052a 100644 --- a/cpp/ChordalBayesNet.h +++ b/cpp/ChordalBayesNet.h @@ -10,95 +10,37 @@ #pragma once #include -#include -#include #include "ConditionalGaussian.h" -#include "Testable.h" +#include "BayesChain.h" namespace gtsam { /** Chordal Bayes Net, the result of eliminating a factor graph */ -class ChordalBayesNet : public Testable +class ChordalBayesNet : public BayesChain { public: typedef boost::shared_ptr shared_ptr; -protected: - - /** nodes keys stored in topological sort order, i.e. from parents to children */ - std::list keys; - - /** nodes stored on key */ - typedef std::map Nodes; - Nodes nodes; - - typedef Nodes::iterator iterator; - -public: - /** Construct an empty net */ ChordalBayesNet() {} /** Copy Constructor */ - ChordalBayesNet(const ChordalBayesNet& cbn_in) : keys(cbn_in.keys), nodes(cbn_in.nodes) {} +// ChordalBayesNet(const ChordalBayesNet& cbn_in) : +// keys_(cbn_in.keys_), nodes_(cbn_in.nodes_) {} /** Destructor */ virtual ~ChordalBayesNet() {} - /** print */ - void print(const std::string& s="") const; - - /** check equality */ - bool equals(const ChordalBayesNet& cbn, double tol=1e-9) const; - - /** insert: use reverse topological sort (i.e. parents last) */ - void insert(const std::string& key, ConditionalGaussian::shared_ptr node); - - /** delete */ - void erase(const std::string& key); - - /** return node with given key */ - inline ConditionalGaussian::shared_ptr get (const std::string& key) const { - const_iterator cg = nodes.find(key); // get node - assert( cg != nodes.end() ); - return cg->second; - } - - inline ConditionalGaussian::shared_ptr operator[](const std::string& key) const { - const_iterator cg = nodes.find(key); // get node - assert( cg != nodes.end() ); - return cg->second; - } - - /** return begin and end of the nodes. FD: breaks encapsulation? */ - typedef Nodes::const_iterator const_iterator; - const_iterator const begin() const {return nodes.begin();} - const_iterator const end() const {return nodes.end();} - /** * optimize, i.e. return x = inv(R)*d */ boost::shared_ptr optimize() const; - /** size is the number of nodes */ - size_t size() const {return nodes.size();} - /** * Return (dense) upper-triangular matrix representation */ std::pair matrix() const; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(keys); - ar & BOOST_SERIALIZATION_NVP(nodes); - } - }; } /// namespace gtsam diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 7f3beda8d..c6b0ac855 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -15,10 +15,15 @@ #include "ChordalBayesNet.h" #include "FactorGraph-inl.h" #include "LinearFactorGraph.h" +#include "SymbolicBayesChain-inl.h" using namespace std; using namespace gtsam; +// explicitly instantiate conversion from LinearFG to SymbolicFG +template SymbolicBayesChain::SymbolicBayesChain + (FactorGraph const&, Ordering const&); + /* ************************************************************************* */ LinearFactorGraph::LinearFactorGraph(const ChordalBayesNet& CBN) { diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 6185f6404..ee997020c 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -37,7 +37,6 @@ external_libs: @echo Compiling CppUnitLite...; cd ../CppUnitLite; make all @echo Compiling Colamd...; cd ../colamd; make all - clean : clean_external_libs @echo Cleaning Cpp...; rm -f *.o *.lo *.*~ libgtsam.la $(check_PROGRAMS) @@ -162,8 +161,8 @@ testVSLAMFactor_LDADD = libgtsam.la # The header files will be installed in ~/include/gtsam -headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h Point2Prior.h -headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h +headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h BayesChain.h +headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += $(sources:.cpp=.h) # templates: headers += FactorGraph.h FactorGraph-inl.h diff --git a/cpp/SymbolicBayesChain.cpp b/cpp/SymbolicBayesChain.cpp index 79c4cbd87..bea17718b 100644 --- a/cpp/SymbolicBayesChain.cpp +++ b/cpp/SymbolicBayesChain.cpp @@ -10,13 +10,16 @@ // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -#include "BayesChain-inl.h" #include "SymbolicBayesChain.h" +#include "BayesChain-inl.h" using namespace std; namespace gtsam { + // Explicitly instantiate so we don't have to include everywhere + template class BayesChain; + typedef pair pp; /* ************************************************************************* */ diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index d11972a0b..67f2dac6d 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -8,7 +8,6 @@ #include #include "Ordering.h" #include "SymbolicFactorGraph.h" -#include "BayesChain-inl.h" #include "SymbolicBayesChain.h" using namespace std; diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 65a1b838c..069aeee6f 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -11,17 +11,12 @@ #pragma once #include -//#include "ConstrainedNonlinearFactorGraph.h" // will be added back once design is solidified -#include "ConstrainedLinearFactorGraph.h" #include "NonlinearFactorGraph.h" -#include "ChordalBayesNet.h" -#include "LinearFactorGraph.h" -#include "VectorConfig.h" - -// \namespace namespace gtsam { + class ConstrainedLinearFactorGraph; + typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; /** diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index b43d95e6d..8b69d89fe 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -6,8 +6,7 @@ #include -#include "BayesChain-inl.h" -#include "SymbolicBayesChain-inl.h" +#include "SymbolicBayesChain.h" #include "smallExample.h" #include "BayesTree.h" diff --git a/cpp/testChordalBayesNet.cpp b/cpp/testChordalBayesNet.cpp index d35548800..1cc37f382 100644 --- a/cpp/testChordalBayesNet.cpp +++ b/cpp/testChordalBayesNet.cpp @@ -20,8 +20,6 @@ #include "ChordalBayesNet.h" #include "smallExample.h" - - using namespace gtsam; /* ************************************************************************* */ diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index fdca93fa6..c4dd4834e 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -42,7 +42,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) Matrix Ay1 = eye(2) * 10; Vector b2 = Vector_(2, 1.0, 2.0); ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1); - CHECK(expectedCCG1.equals(*(cbn->get("x")))); + CHECK(expectedCCG1.equals(*((*cbn)["x"]))); // verify remaining factor on y // Gaussian factor on X becomes different Gaussian factor on Y @@ -66,7 +66,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) R(1, 0) = 0.0; R(1, 1) = 44.7214; Vector br = Vector_(2, 8.9443, 4.4721); ConditionalGaussian expected2(br, R); - CHECK(expected2.equals(*(cbn->get("y")))); + CHECK(expected2.equals(*((*cbn)["y"]))); } /* ************************************************************************* */ diff --git a/cpp/testSymbolicBayesChain.cpp b/cpp/testSymbolicBayesChain.cpp index 9837c4bb1..b27b2c893 100644 --- a/cpp/testSymbolicBayesChain.cpp +++ b/cpp/testSymbolicBayesChain.cpp @@ -8,8 +8,7 @@ #include "smallExample.h" #include "FactorGraph-inl.h" -#include "BayesChain-inl.h" -#include "SymbolicBayesChain-inl.h" +#include "SymbolicBayesChain.h" using namespace std; using namespace gtsam; diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 761d91b98..c4688f5b3 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -9,7 +9,6 @@ #include "smallExample.h" #include "FactorGraph-inl.h" #include "SymbolicFactorGraph.h" -#include "BayesChain-inl.h" #include "SymbolicConditional.h" #include "SymbolicBayesChain.h"