diff --git a/.cproject b/.cproject index 739ad24a7..093b27c0a 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -316,6 +314,7 @@ make + check true true @@ -323,7 +322,6 @@ make - testSimpleCamera.run true true @@ -339,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -346,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -354,6 +352,7 @@ make + testConditionalGaussian.run true true @@ -361,7 +360,6 @@ make - testPose2.run true true @@ -377,7 +375,6 @@ make - testRot3.run true true @@ -385,6 +382,7 @@ make + testNonlinearOptimizer.run true true @@ -392,7 +390,6 @@ make - testLinearFactor.run true true @@ -400,7 +397,6 @@ make - testConstrainedNonlinearFactorGraph.run true true @@ -408,7 +404,6 @@ make - testLinearFactorGraph.run true true @@ -416,6 +411,7 @@ make + testNonlinearFactorGraph.run true true @@ -423,7 +419,6 @@ make - testPose3.run true true @@ -431,6 +426,7 @@ make + testConstrainedLinearFactorGraph.run true true @@ -438,6 +434,7 @@ make + testVectorConfig.run true true @@ -445,6 +442,7 @@ make + testPoint2.run true true @@ -452,7 +450,6 @@ make - testNonlinearFactor.run true true @@ -460,7 +457,6 @@ make - timeLinearFactor.run true true @@ -468,22 +464,21 @@ make - timeLinearFactorGraph.run true true true - + make - -testChordalBayesNet.run +testGaussianBayesNet.run true true true make + testBayesTree.run true false @@ -491,6 +486,7 @@ make + testSymbolicBayesChain.run true false @@ -498,6 +494,7 @@ make + testSymbolicFactorGraph.run true false @@ -505,7 +502,6 @@ make - testVector.run true true @@ -513,7 +509,6 @@ make - testMatrix.run true true @@ -521,6 +516,7 @@ make + install true true @@ -528,6 +524,7 @@ make + clean true true @@ -535,6 +532,7 @@ make + check true true diff --git a/cpp/BayesNet-inl.h b/cpp/BayesNet-inl.h index f87501103..ada9d0a08 100644 --- a/cpp/BayesNet-inl.h +++ b/cpp/BayesNet-inl.h @@ -8,49 +8,42 @@ #include #include +#include // for += +using namespace boost::assign; + +#include "Ordering.h" #include "BayesNet.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 BayesNet::print(const string& s) const { cout << s << ":\n"; - BOOST_FOREACH(string key, keys_) { - const_iterator it = nodes_.find(key); - it->second->print("Node[" + key + "]"); - } + std::string key; + BOOST_FOREACH(conditional_ptr conditional,conditionals_) + conditional->print("Node[" + conditional->key() + "]"); } /* ************************************************************************* */ template bool BayesNet::equals(const BayesNet& cbn, double tol) const { + if(indices_ != cbn.indices_) return false; if(size() != cbn.size()) return false; - if(keys_ != cbn.keys_) return false; - string key; - boost::shared_ptr node; - FOREACH_PAIR( key, node, nodes_) { - const_iterator cg = cbn.nodes_.find(key); - if (cg == nodes_.end()) return false; - if (!equals_star(node,cg->second,tol)) return false; - } - return true; + return equal(conditionals_.begin(),conditionals_.begin(),conditionals_.begin(),equals_star); } /* ************************************************************************* */ template void BayesNet::insert - (const string& key, boost::shared_ptr node) { - keys_.push_front(key); - nodes_.insert(make_pair(key,node)); + (const boost::shared_ptr& conditional) { + indices_.insert(make_pair(conditional->key(),conditionals_.size())); + conditionals_.push_back(conditional); } - /* ************************************************************************* */ + /* ************************************************************************* * template void BayesNet::erase(const string& key) { list::iterator it; @@ -59,9 +52,18 @@ namespace gtsam { break; } keys_.erase(it); - nodes_.erase(key); + conditionals_.erase(key); } -/* ************************************************************************* */ + /* ************************************************************************* */ + template + Ordering BayesNet::ordering() const { + Ordering ord; + BOOST_FOREACH(conditional_ptr conditional,conditionals_) + ord.push_back(conditional->key()); + return ord; + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/BayesNet.h b/cpp/BayesNet.h index b775a57d0..4a9afed33 100644 --- a/cpp/BayesNet.h +++ b/cpp/BayesNet.h @@ -8,14 +8,18 @@ #pragma once +#include #include #include -#include +#include +#include #include "Testable.h" namespace gtsam { + class Ordering; + /** * Bayes network * This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet @@ -24,14 +28,36 @@ namespace gtsam { */ template class BayesNet: public Testable > { + + public: + + /** We store shared pointers to Conditional densities */ + typedef typename boost::shared_ptr conditional_ptr; + typedef typename std::vector Conditionals; + typedef typename Conditionals::const_iterator const_iterator; + typedef typename Conditionals::const_reverse_iterator const_reverse_iterator; + protected: - /** nodes keys stored in topological sort order, i.e. from parents to children */ - std::list keys_; + /** + * Conditional densities are stored in reverse topological sort order (i.e., leaves first, + * parents last), which corresponds to the elimination ordering if so obtained, + * and is consistent with the column (block) ordering of an upper triangular matrix. + */ + Conditionals conditionals_; - /** nodes stored on key */ - typedef typename std::map > Nodes; - Nodes nodes_; + /** + * O(log n) random access on keys will provided by a map from keys to vector index. + */ + typedef std::map Indices; + Indices indices_; + + /** O(log n) lookup from key to node index */ + inline int index(const std::string& key) const { + Indices::const_iterator it = indices_.find(key); // get node index + assert( it != indices_.end() ); + return it->second; + } public: @@ -41,37 +67,36 @@ namespace gtsam { /** check equality */ bool equals(const BayesNet& 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); - - /** delete */ - void erase(const std::string& key); + /** insert: use reverse topological sort (i.e. parents last / elimination order) */ + void insert(const boost::shared_ptr& conditional); /** size is the number of nodes */ - inline size_t size() const {return nodes_.size();} - - /** return keys in topological sort order (parents first), i.e., reverse elimination order */ - inline std::list keys() const { return keys_;} - - 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; + inline size_t size() const { + return conditionals_.size(); } - /** 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();} + /** return keys in reverse topological sort order, i.e., elimination order */ + Ordering ordering() const; + + /** O(log n) random access to Conditional by key */ + inline conditional_ptr operator[](const std::string& key) const { + int i = index(key); + return conditionals_[i]; + } + + /** return iterators. FD: breaks encapsulation? */ + const_iterator const begin() const {return conditionals_.begin();} + const_iterator const end() const {return conditionals_.end();} + const_reverse_iterator const rbegin() const {return conditionals_.rbegin();} + const_reverse_iterator const rend() const {return conditionals_.rend();} 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_); + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(conditionals_); + ar & BOOST_SERIALIZATION_NVP(indices_); } }; diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 37c0f56af..f1897900c 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -13,8 +13,8 @@ namespace gtsam { /* ************************************************************************* */ template - Front::Front(string key, cond_ptr conditional) { - add(key, conditional); + Front::Front(const conditional_ptr& conditional) { + add(conditional); separator_ = conditional->parents(); } @@ -22,11 +22,12 @@ namespace gtsam { template void Front::print(const string& s) const { cout << s; - BOOST_FOREACH(string key, keys_) cout << " " << key; + BOOST_FOREACH(const conditional_ptr& conditional, conditionals_) + cout << " " << conditional->key(); if (!separator_.empty()) { cout << " :"; BOOST_FOREACH(string key, separator_) - cout << " " << key; + cout << " " << key; } cout << endl; } @@ -34,14 +35,12 @@ namespace gtsam { /* ************************************************************************* */ template bool Front::equals(const Front& other, double tol) const { - return (keys_ == other.keys_) && - equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star); + return equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star); } /* ************************************************************************* */ template - void Front::add(string key, cond_ptr conditional) { - keys_.push_front(key); + void Front::add(const conditional_ptr& conditional) { conditionals_.push_front(conditional); } @@ -51,12 +50,13 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: traversal is O(n*log(n)) but could be O(n) with better bayesChain + // TODO: traversal is O(n*log(n)) but could be O(n) with better bayesNet template - BayesTree::BayesTree(BayesNet& bayesChain, bool verbose) { - list reverseOrdering = bayesChain.keys(); - BOOST_FOREACH(string key, reverseOrdering) - insert(key,bayesChain[key],verbose); + BayesTree::BayesTree(const BayesNet& bayesNet, bool verbose) { + typename BayesNet::const_reverse_iterator rit; + for ( rit=bayesNet.rbegin(); rit < bayesNet.rend(); ++rit ) { + insert(*rit,verbose); + } } /* ************************************************************************* */ @@ -78,8 +78,9 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::insert(string key, conditional_ptr conditional, bool verbose) { + void BayesTree::insert(const boost::shared_ptr& conditional, bool verbose) { + string key = conditional->key(); if (verbose) cout << "Inserting " << key << "| "; // get parents @@ -90,7 +91,7 @@ namespace gtsam { // if no parents, start a new root clique if (parents.empty()) { if (verbose) cout << "Creating root clique" << endl; - node_ptr root(new Node(key, conditional)); + node_ptr root(new Node(conditional)); nodes_.push_back(root); nodeMap_.insert(make_pair(key, 0)); return; @@ -108,13 +109,13 @@ namespace gtsam { if (parent_clique->size() == parents.size()) { if (verbose) cout << "Adding to clique " << index << endl; nodeMap_.insert(make_pair(key, index)); - parent_clique->add(key, conditional); + parent_clique->add(conditional); return; } // otherwise, start a new clique and add it to the tree if (verbose) cout << "Starting new clique" << endl; - node_ptr new_clique(new Node(key, conditional)); + node_ptr new_clique(new Node(conditional)); new_clique->parent_ = parent_clique; parent_clique->children_.push_back(new_clique); nodeMap_.insert(make_pair(key, nodes_.size())); @@ -123,4 +124,5 @@ namespace gtsam { /* ************************************************************************* */ -} /// namespace gtsam +} +/// namespace gtsam diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 25cc73155..1c4aeb8a4 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -22,14 +22,13 @@ namespace gtsam { template class Front: Testable > { private: - typedef boost::shared_ptr cond_ptr; - std::list keys_; /** frontal keys */ - std::list conditionals_; /** conditionals */ + typedef boost::shared_ptr conditional_ptr; + std::list conditionals_; /** conditionals */ std::list separator_; /** separator keys */ public: /** constructor */ - Front(std::string key, cond_ptr conditional); + Front(const conditional_ptr& conditional); /** print */ void print(const std::string& s = "") const; @@ -38,10 +37,10 @@ namespace gtsam { bool equals(const Front& other, double tol = 1e-9) const; /** add a frontal node */ - void add(std::string key, cond_ptr conditional); + void add(const conditional_ptr& conditional); /** return size of the clique */ - inline size_t size() const {return keys_.size() + separator_.size();} + inline size_t size() const {return conditionals_.size() + separator_.size();} }; /** @@ -55,6 +54,7 @@ namespace gtsam { public: typedef boost::shared_ptr conditional_ptr; + typedef std::pair NamedConditional; private: @@ -64,7 +64,7 @@ namespace gtsam { shared_ptr parent_; std::list children_; - Node(std::string key, conditional_ptr conditional):Front(key,conditional) {} + Node(const boost::shared_ptr& conditional):Front(conditional) {} /** print this node and entire subtree below it*/ void printTree(const std::string& indent) const { @@ -88,8 +88,8 @@ namespace gtsam { /** Create an empty Bayes Tree */ BayesTree(); - /** Create a Bayes Tree from a SymbolicBayesNet */ - BayesTree(BayesNet& bayesChain, bool verbose=false); + /** Create a Bayes Tree from a Bayes Net */ + BayesTree(const BayesNet& bayesNet, bool verbose=false); /** Destructor */ virtual ~BayesTree() {} @@ -101,9 +101,9 @@ namespace gtsam { bool equals(const BayesTree& other, double tol = 1e-9) const; /** insert a new conditional */ - void insert(std::string key, conditional_ptr conditional, bool verbose=false); + void insert(const boost::shared_ptr& conditional, bool verbose=false); - /** number of cliques */ + /** number of cliques */ inline size_t size() const { return nodes_.size();} /** return root clique */ diff --git a/cpp/Conditional.h b/cpp/Conditional.h new file mode 100644 index 000000000..03e229c9c --- /dev/null +++ b/cpp/Conditional.h @@ -0,0 +1,52 @@ +/** + * @file Conditional.h + * @brief Base class for conditional densities + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include // for noncopyable +#include "Testable.h" + +namespace gtsam { + + /** + * Base class for conditional densities + * + * We make it noncopyable so we enforce the fact that factors are + * kept in pointer containers. To be safe, you should make them + * immutable, i.e., practicing functional programming. + */ + class Conditional : boost::noncopyable, public Testable + { + protected: + + /** key of random variable */ + std::string key_; + + public: + + /** constructor */ + Conditional(const std::string& key):key_(key) {} + + /* destructor */ + virtual ~Conditional() {}; + + /** check equality */ + bool equals(const Conditional& c, double tol = 1e-9) const { + return key_ == c.key_; + } + + /** return key */ + inline const std::string& key() const { return key_;} + + /** return parent keys */ + virtual std::list parents() const = 0; + + /** return the number of parents */ + virtual std::size_t nrParents() const = 0; + }; +} diff --git a/cpp/ConditionalGaussian.cpp b/cpp/ConditionalGaussian.cpp index f480a9874..4f962046c 100644 --- a/cpp/ConditionalGaussian.cpp +++ b/cpp/ConditionalGaussian.cpp @@ -4,49 +4,39 @@ * @author Christian Potthast */ - #include #include +#include "Ordering.h" #include "ConditionalGaussian.h" using namespace std; using namespace gtsam; /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(Vector d,Matrix R) : R_(R),d_(d) -{ +ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R) : + Conditional (key), R_(R), d_(d) { } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(Vector d, - Matrix R, - const string& name1, - Matrix S) - : R_(R),d_(d) -{ - parents_.insert(make_pair(name1, S)); +ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, + const string& name1, Matrix S) : + Conditional (key), R_(R), d_(d) { + parents_.insert(make_pair(name1, S)); } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(Vector d, - Matrix R, - const string& name1, - Matrix S, - const string& name2, - Matrix T) - : R_(R),d_(d) -{ - parents_.insert(make_pair(name1, S)); - parents_.insert(make_pair(name2, T)); +ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, + const string& name1, Matrix S, const string& name2, Matrix T) : + Conditional (key), R_(R), d_(d) { + parents_.insert(make_pair(name1, S)); + parents_.insert(make_pair(name2, T)); } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(const Vector& d, - const Matrix& R, - const map& parents) - : R_(R), d_(d), parents_(parents) - { - } +ConditionalGaussian::ConditionalGaussian(const string& key, + const Vector& d, const Matrix& R, const map& parents) : + Conditional (key), R_(R), d_(d), parents_(parents) { +} /* ************************************************************************* */ void ConditionalGaussian::print(const string &s) const @@ -62,23 +52,26 @@ void ConditionalGaussian::print(const string &s) const } /* ************************************************************************* */ -bool ConditionalGaussian::equals(const ConditionalGaussian &cg, double tol) const { +bool ConditionalGaussian::equals(const Conditional &c, double tol) const { + if (!Conditional::equals(c)) return false; + const ConditionalGaussian* p = dynamic_cast (&c); + if (p == NULL) return false; Parents::const_iterator it = parents_.begin(); // check if the size of the parents_ map is the same - if (parents_.size() != cg.parents_.size()) return false; + if (parents_.size() != p->parents_.size()) return false; // check if R_ is equal - if (!(equal_with_abs_tol(R_, cg.R_, tol))) return false; + if (!(equal_with_abs_tol(R_, p->R_, tol))) return false; // check if d_ is equal - if (!(::equal_with_abs_tol(d_, cg.d_, tol))) return false; + if (!(::equal_with_abs_tol(d_, p->d_, tol))) return false; // check if the matrices are the same // iterate over the parents_ map for (it = parents_.begin(); it != parents_.end(); it++) { - Parents::const_iterator it2 = cg.parents_.find(it->first.c_str()); - if (it2 != cg.parents_.end()) { + Parents::const_iterator it2 = p->parents_.find(it->first.c_str()); + if (it2 != p->parents_.end()) { if (!(equal_with_abs_tol(it->second, it2->second, tol))) return false; } else return false; diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h index 2dbdacfe0..53de0b658 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/ConditionalGaussian.h @@ -4,7 +4,6 @@ * @author Christian Potthast */ - // \callgraph #pragma once @@ -14,128 +13,131 @@ #include #include #include +#include #include -#include "Matrix.h" + +#include "Conditional.h" #include "VectorConfig.h" -#include "Ordering.h" -#include "Testable.h" +#include "Matrix.h" namespace gtsam { - - /** - * A conditional Gaussian functions as the node in a Bayes network - * It has a set of parents y,z, etc. and implements a probability density on x. - * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2 - */ - class ConditionalGaussian : boost::noncopyable, public Testable - { - public: - typedef std::map Parents; - typedef Parents::const_iterator const_iterator; - typedef boost::shared_ptr shared_ptr; - - protected: - /** the triangular matrix (square root information matrix) */ - Matrix R_; + class Ordering; - /** the names and the matrices connecting to parent nodes */ - Parents parents_; + /** + * A conditional Gaussian functions as the node in a Bayes network + * It has a set of parents y,z, etc. and implements a probability density on x. + * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2 + */ + class ConditionalGaussian: public Conditional { + public: + typedef std::map Parents; + typedef Parents::const_iterator const_iterator; + typedef boost::shared_ptr shared_ptr; - /** the RHS vector */ - Vector d_; + protected: - public: + /** the triangular matrix (square root information matrix) */ + Matrix R_; - /** constructor */ - ConditionalGaussian() {}; - - /** Copy Constructor */ - ConditionalGaussian(const ConditionalGaussian &cg) : - boost::noncopyable(), R_(cg.R_), parents_(cg.parents_), d_(cg.d_){} - - /** constructor with no parents - * |Rx-d| - */ - ConditionalGaussian(Vector d, - Matrix R); - - /** constructor with only one parent - * |Rx+Sy-d| - */ - ConditionalGaussian(Vector d, - Matrix R, - const std::string& name1, - Matrix S - ); - - /** constructor with two parents - * |Rx+Sy+Tz-d| - */ - ConditionalGaussian(Vector d, - Matrix R, - const std::string& name1, - Matrix S, - const std::string& name2, - Matrix T - ); + /** the names and the matrices connecting to parent nodes */ + Parents parents_; - /** - * constructor with number of arbitrary parents - * |Rx+sum(Ai*xi)-d| - */ - ConditionalGaussian(const Vector& d, - const Matrix& R, - const Parents& parents); + /** the RHS vector */ + Vector d_; - /** deconstructor */ - virtual ~ConditionalGaussian() {}; + public: - /** print */ - void print(const std::string& = "ConditionalGaussian") const; + /** default constructor needed for serialization */ + ConditionalGaussian():Conditional("__unitialized__") {} - /** equals function */ - bool equals(const ConditionalGaussian &cg, double tol=1e-9) const; + /** constructor */ + ConditionalGaussian(const std::string& key) : + Conditional (key) {} - /** dimension of multivariate variable */ - size_t dim() const {return R_.size2();} + /** constructor with no parents + * |Rx-d| + */ + ConditionalGaussian(const std::string& key, Vector d, Matrix R); - /** return all parents */ - std::list parents() const; + /** constructor with only one parent + * |Rx+Sy-d| + */ + ConditionalGaussian(const std::string& key, Vector d, Matrix R, + const std::string& name1, Matrix S); - /** return stuff contained in ConditionalGaussian */ - const Vector& get_d() const {return d_;} - const Matrix& get_R() const {return R_;} + /** constructor with two parents + * |Rx+Sy+Tz-d| + */ + ConditionalGaussian(const std::string& key, Vector d, Matrix R, + const std::string& name1, Matrix S, const std::string& name2, Matrix T); - /** STL like, return the iterator pointing to the first node */ - const_iterator const parentsBegin() const { return parents_.begin(); } + /** + * constructor with number of arbitrary parents + * |Rx+sum(Ai*xi)-d| + */ + ConditionalGaussian(const std::string& key, const Vector& d, + const Matrix& R, const Parents& parents); - /** STL like, return the iterator pointing to the last node */ - const_iterator const parentsEnd() const { return parents_.end(); } + /** deconstructor */ + virtual ~ConditionalGaussian() {} - /** find the number of parents */ - size_t size() const {return parents_.size();} + /** print */ + void print(const std::string& = "ConditionalGaussian") const; - /** determine whether a key is among the parents */ - size_t contains(const std::string& key) const {return parents_.count(key);} + /** equals function */ + bool equals(const Conditional &cg, double tol = 1e-9) const; - /** - * solve a conditional Gaussian - * @param x configuration in which the parents values (y,z,...) are known - * @return solution x = R \ (d - Sy - Tz - ...) - */ - virtual Vector solve(const VectorConfig& x) const; + /** dimension of multivariate variable */ + size_t dim() const { return R_.size2();} - /** - * adds a parent - */ - void add(const std::string key, Matrix S){ parents_.insert(make_pair(key, S)); } - - private: + /** return all parents */ + std::list parents() const; + + /** return stuff contained in ConditionalGaussian */ + const Vector& get_d() const { return d_;} + const Matrix& get_R() const { return R_;} + + /** STL like, return the iterator pointing to the first node */ + const_iterator const parentsBegin() const { + return parents_.begin(); + } + + /** STL like, return the iterator pointing to the last node */ + const_iterator const parentsEnd() const { + return parents_.end(); + } + + /** find the number of parents */ + size_t nrParents() const { + return parents_.size(); + } + + /** determine whether a key is among the parents */ + size_t contains(const std::string& key) const { + return parents_.count(key); + } + + /** + * solve a conditional Gaussian + * @param x configuration in which the parents values (y,z,...) are known + * @return solution x = R \ (d - Sy - Tz - ...) + */ + virtual Vector solve(const VectorConfig& x) const; + + /** + * adds a parent + */ + void add(const std::string key, Matrix S) { + parents_.insert(make_pair(key, S)); + } + + private: /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(key_); ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(d_); ar & BOOST_SERIALIZATION_NVP(parents_); diff --git a/cpp/ConstrainedConditionalGaussian.cpp b/cpp/ConstrainedConditionalGaussian.cpp index 0abec005a..63056e11b 100644 --- a/cpp/ConstrainedConditionalGaussian.cpp +++ b/cpp/ConstrainedConditionalGaussian.cpp @@ -12,40 +12,39 @@ using namespace gtsam; using namespace std; -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian() { +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const string& key) : + ConditionalGaussian(key) { } -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& v) : - ConditionalGaussian(v, eye(v.size())) { -} - -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, - const Matrix& A) : - ConditionalGaussian(b, A) { -} - -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, - const Matrix& A1, const std::string& parent, const Matrix& A2) : - ConditionalGaussian(b, A1, parent, A2) { -} - -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, - const Matrix& A1, const std::string& parentY, const Matrix& A2, - const std::string& parentZ, const Matrix& A3) -: ConditionalGaussian(b, A1, parentY, A2, parentZ, A3) -{ -} - -ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Matrix& A1, - const std::map& parents, const Vector& b) -: ConditionalGaussian(b, A1, parents) -{ -} - - ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( - const ConstrainedConditionalGaussian& df) { + const string& key, const Vector& v) : + ConditionalGaussian(key, v, eye(v.size())) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const string& key, const Vector& b, const Matrix& A) : + ConditionalGaussian(key, b, A) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const string& key, const Vector& b, const Matrix& A1, + const std::string& parent, const Matrix& A2) : + ConditionalGaussian(key, b, A1, parent, A2) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const string& key, const Vector& b, const Matrix& A1, + const std::string& parentY, const Matrix& A2, const std::string& parentZ, + const Matrix& A3) : + ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const string& key, const Matrix& A1, + const std::map& parents, const Vector& b) : + ConditionalGaussian(key, b, A1, parents) { } Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const { diff --git a/cpp/ConstrainedConditionalGaussian.h b/cpp/ConstrainedConditionalGaussian.h index fff94ca17..95ab7114f 100644 --- a/cpp/ConstrainedConditionalGaussian.h +++ b/cpp/ConstrainedConditionalGaussian.h @@ -33,14 +33,14 @@ public: * Default Constructor * Don't use this */ - ConstrainedConditionalGaussian(); + ConstrainedConditionalGaussian(const std::string& key); /** * Used for unary factors that simply associate a name with a particular value * Can use backsubstitution to solve trivially * @param value is a fixed value for x in the form x = value */ - ConstrainedConditionalGaussian(const Vector& value); + ConstrainedConditionalGaussian(const std::string& key, const Vector& value); /** * Used for unary factors of the form Ax=b @@ -48,7 +48,7 @@ public: * @param b is the RHS of the equation * @param A is the A matrix */ - ConstrainedConditionalGaussian(const Vector& value, const Matrix& A); + ConstrainedConditionalGaussian(const std::string& key, const Vector& value, const Matrix& A); /** * Binary constructor of the form A1*x = b - A2*y @@ -58,7 +58,7 @@ public: * @param parent is the string identifier for the parent node * @param A2 is the A2 matrix */ - ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1, + ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1, const std::string& parent, const Matrix& A2); /** @@ -70,7 +70,7 @@ public: * @param parentZ string id for z * @param A3 is the A3 matrix */ - ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1, + ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1, const std::string& parentY, const Matrix& A2, const std::string& parentZ, const Matrix& A3); @@ -81,14 +81,9 @@ public: * @param parents is the map of parents (Ai and xi from above) * @param b is the rhs vector */ - ConstrainedConditionalGaussian(const Matrix& A1, + ConstrainedConditionalGaussian(const std::string& key, const Matrix& A1, const std::map& parents, const Vector& b); - /** - * Copy constructor - */ - ConstrainedConditionalGaussian(const ConstrainedConditionalGaussian& df); - virtual ~ConstrainedConditionalGaussian() { } diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 9ce40e07c..cc20d9b17 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -6,6 +6,7 @@ #include #include #include +#include "Ordering.h" #include "ConstrainedLinearFactorGraph.h" using namespace std; @@ -78,12 +79,12 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order if (is_constrained(key)) { ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key); - cbn->insert(key,ccg); + cbn->insert(ccg); } else { ConditionalGaussian::shared_ptr cg = eliminateOne(key); - cbn->insert(key,cg); + cbn->insert(cg); } } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index ffc325c24..aecbc49c6 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -11,6 +11,7 @@ #include #include +#include #include #include diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index 08cae1c1c..ffb27ff06 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -20,6 +20,7 @@ template class BayesNet; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) /* ************************************************************************* */ boost::shared_ptr GaussianBayesNet::optimize() const @@ -27,11 +28,9 @@ boost::shared_ptr GaussianBayesNet::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 - Vector x = cg->second->solve(*result); // Solve for that variable - result->insert(key,x); // store result in partial solution + BOOST_REVERSE_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) { + Vector x = cg->solve(*result); // Solve for that variable + result->insert(cg->key(),x); // store result in partial solution } return result; } @@ -41,22 +40,19 @@ pair GaussianBayesNet::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_) { - // find corresponding node - const_iterator it = nodes_.find(key); - indices.insert(make_pair(key,N)); - N += it->second->dim(); + size_t N=0; map mapping; + BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) { + mapping.insert(make_pair(cg->key(),N)); + N += cg->dim(); } // create matrix and copy in values Matrix R = zeros(N,N); Vector d(N); - string key; size_t I; - FOREACH_PAIR(key,I,indices) { - // find corresponding node - const_iterator it = nodes_.find(key); - ConditionalGaussian::shared_ptr cg = it->second; + string key; size_t I; + FOREACH_PAIR(key,I,mapping) { + // find corresponding conditional + ConditionalGaussian::shared_ptr cg = (*this)[key]; // get RHS and copy to d const Vector& d_ = cg->get_d(); @@ -68,17 +64,17 @@ pair GaussianBayesNet::matrix() const { const Matrix& R_ = cg->get_R(); for (size_t i=0;iparentsBegin(); for (; keyS!=cg->parentsEnd(); keyS++) { Matrix S = keyS->second; // get S matrix const size_t m = S.size1(), n = S.size2(); // find S size - const size_t J = indices[keyS->first]; // find column index + const size_t J = mapping[keyS->first]; // find column index for (size_t i=0;i #include + +#include "Matrix.h" +#include "Ordering.h" +#include "ConditionalGaussian.h" #include "LinearFactor.h" using namespace std; @@ -20,6 +24,18 @@ using namespace gtsam; typedef pair& mypair; +/* ************************************************************************* */ +LinearFactor::LinearFactor(const boost::shared_ptr cg) : + b(cg->get_d()) { + As.insert(make_pair(cg->key(), cg->get_R())); + std::map::const_iterator it = cg->parentsBegin(); + for (; it != cg->parentsEnd(); it++) { + const std::string& j = it->first; + const Matrix& Aj = it->second; + As.insert(make_pair(j, Aj)); + } +} + /* ************************************************************************* */ LinearFactor::LinearFactor(const vector & factors) { @@ -186,7 +202,7 @@ LinearFactor::eliminate(const string& key) // if this factor does not involve key, we exit with empty CG and LF if (it==As.end()) { // Conditional Gaussian is just a parent-less node with P(x)=1 - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian); + ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key)); return make_pair(cg,lf); } @@ -227,7 +243,7 @@ LinearFactor::eliminate(const string& key) } // column j // create ConditionalGaussian with first n rows - ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(::sub(b,0,n), sub(R,0,n,0,n)) ); + ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(key,::sub(b,0,n), sub(R,0,n,0,n)) ); // create linear factor with remaining rows lf->set_b(::sub(b,n,m)); diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index 815d4dfe4..65bf22ff8 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -9,23 +9,18 @@ #pragma once -#include -#include -#include -#include #include -#include +#include -#include "Matrix.h" #include "Factor.h" -#include "LinearFactorSet.h" -#include "ConditionalGaussian.h" -#include "Ordering.h" - -#define CONSTRUCTOR +#include "Matrix.h" +#include "VectorConfig.h" namespace gtsam { + class ConditionalGaussian; + class Ordering; + /** * Base Class for a linear factor. * LinearFactor is non-mutable (all methods const!). @@ -50,20 +45,17 @@ public: } /** Construct Null factor */ - CONSTRUCTOR LinearFactor(const Vector& b_in) : b(b_in) { //TODO: add a way to initializing base class meaningfully } /** Construct unary factor */ - CONSTRUCTOR LinearFactor(const std::string& key1, const Matrix& A1, const Vector& b_in) : b(b_in) { As.insert(make_pair(key1, A1)); } /** Construct binary factor */ - CONSTRUCTOR LinearFactor(const std::string& key1, const Matrix& A1, const std::string& key2, const Matrix& A2, const Vector& b_in) : b(b_in) { @@ -72,7 +64,6 @@ public: } /** Construct ternary factor */ - CONSTRUCTOR LinearFactor(const std::string& key1, const Matrix& A1, const std::string& key2, const Matrix& A2, const std::string& key3, const Matrix& A3, const Vector& b_in) : @@ -83,7 +74,6 @@ public: } /** Construct an n-ary factor */ - CONSTRUCTOR LinearFactor(const std::vector > &terms, const Vector &b_in) : b(b_in) { @@ -92,24 +82,12 @@ public: } /** Construct from Conditional Gaussian */ - CONSTRUCTOR - LinearFactor(const std::string& key, const boost::shared_ptr< - ConditionalGaussian> cg) : - b(cg->get_d()) { - As.insert(make_pair(key, cg->get_R())); - std::map::const_iterator it = cg->parentsBegin(); - for (; it != cg->parentsEnd(); it++) { - const std::string& j = it->first; - const Matrix& Aj = it->second; - As.insert(make_pair(j, Aj)); - } - } + LinearFactor(const boost::shared_ptr cg); /** * Constructor that combines a set of factors * @param factors Set of factors to combine */ - CONSTRUCTOR LinearFactor(const std::vector & factors); // Implementing Testable virtual functions @@ -212,7 +190,7 @@ public: * @param key the key of the node to be eliminated * @return a new factor and a conditional gaussian on the eliminated variable */ - std::pair eliminate(const std::string& key); + std::pair, shared_ptr> eliminate(const std::string& key); /** * Take the factor f, and append to current matrices. Not very general. diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 5e32f1320..3a0e3fd61 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -15,6 +15,7 @@ #include "GaussianBayesNet.h" #include "FactorGraph-inl.h" #include "LinearFactorGraph.h" +#include "LinearFactorSet.h" using namespace std; using namespace gtsam; @@ -34,7 +35,7 @@ void LinearFactorGraph::setCBN(const GaussianBayesNet& CBN) clear(); GaussianBayesNet::const_iterator it = CBN.begin(); for(; it != CBN.end(); it++) { - LinearFactor::shared_ptr lf(new LinearFactor(it->first, it->second)); + LinearFactor::shared_ptr lf(new LinearFactor(*it)); push_back(lf); } } @@ -62,7 +63,7 @@ LinearFactorGraph::eliminate_partially(const Ordering& ordering) BOOST_FOREACH(string key, ordering) { ConditionalGaussian::shared_ptr cg = eliminateOne(key); - chordalBayesNet->insert(key,cg); + chordalBayesNet->insert(cg); } return chordalBayesNet; diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 0df60d8f1..33fe6f82d 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -13,14 +13,14 @@ #pragma once #include - -#include "LinearFactor.h" -#include "VectorConfig.h" #include "FactorGraph.h" -#include "GaussianBayesNet.h" +#include "LinearFactor.h" namespace gtsam { + class Ordering; + class GaussianBayesNet; + /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == LinearFactor diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 92cd98d58..96f3fa705 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -161,7 +161,7 @@ testVSLAMFactor_LDADD = libgtsam.la # The header files will be installed in ~/include/gtsam -headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h +headers = gtsam.h Value.h Testable.h Factor.h Conditional.h LinearFactorSet.h headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += $(sources:.cpp=.h) # templates: diff --git a/cpp/SymbolicConditional.h b/cpp/SymbolicConditional.h index 2c4f35f63..7c1d4b253 100644 --- a/cpp/SymbolicConditional.h +++ b/cpp/SymbolicConditional.h @@ -12,14 +12,14 @@ #include #include #include // TODO: make cpp file -#include "Testable.h" +#include "Conditional.h" namespace gtsam { /** * Conditional node for use in a Bayes net */ - class SymbolicConditional: Testable { + class SymbolicConditional: public Conditional { private: @@ -33,20 +33,24 @@ namespace gtsam { /** * No parents */ - SymbolicConditional() { + SymbolicConditional(const std::string& key) : + Conditional(key) { } /** * Single parent */ - SymbolicConditional(const std::string& parent) { + SymbolicConditional(const std::string& key, const std::string& parent) : + Conditional(key) { parents_.push_back(parent); } /** * Two parents */ - SymbolicConditional(const std::string& parent1, const std::string& parent2) { + SymbolicConditional(const std::string& key, const std::string& parent1, + const std::string& parent2) : + Conditional(key) { parents_.push_back(parent1); parents_.push_back(parent2); } @@ -54,24 +58,34 @@ namespace gtsam { /** * A list */ - SymbolicConditional(const std::list& parents):parents_(parents) { + SymbolicConditional(const std::string& key, + const std::list& parents) : + Conditional(key), parents_(parents) { } /** print */ void print(const std::string& s = "SymbolicConditional") const { - std::cout << s; + std::cout << s << " P(" << key_ << " |"; BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent; - std::cout << std::endl; + std::cout << ")" << std::endl; } /** check equality */ - bool equals(const SymbolicConditional& other, double tol = 1e-9) const { - return parents_ == other.parents_; + bool equals(const Conditional& c, double tol = 1e-9) const { + if (!Conditional::equals(c)) return false; + const SymbolicConditional* p = dynamic_cast (&c); + if (p == NULL) return false; + return parents_ == p->parents_; } - /** return any parent */ + /** return parents */ std::list parents() const { return parents_;} + /** find the number of parents */ + size_t nrParents() const { + return parents_.size(); + } + }; } /// namespace gtsam diff --git a/cpp/SymbolicFactor.cpp b/cpp/SymbolicFactor.cpp index 8c9fc0a2a..63ed9bc31 100644 --- a/cpp/SymbolicFactor.cpp +++ b/cpp/SymbolicFactor.cpp @@ -59,7 +59,7 @@ namespace gtsam { boost::shared_ptr lf(new SymbolicFactor(separator)); // create SymbolicConditional on separator - SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator)); + SymbolicConditional::shared_ptr cg (new SymbolicConditional(key,separator)); return make_pair(cg,lf); } diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index 0982fc252..e305d8273 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -22,14 +22,14 @@ namespace gtsam { SymbolicBayesNet::shared_ptr SymbolicFactorGraph::eliminate(const Ordering& ordering) { - SymbolicBayesNet::shared_ptr bayesChain (new SymbolicBayesNet()); + SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet()); BOOST_FOREACH(string key, ordering) { SymbolicConditional::shared_ptr conditional = eliminateOne(key); - bayesChain->insert(key,conditional); + bayesNet->insert(conditional); } - return bayesChain; + return bayesNet; } /* ************************************************************************* */ diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 7eb7b25c9..fc056ef05 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -200,11 +200,11 @@ GaussianBayesNet createSmallGaussianBayesNet() // define nodes and specify in reverse topological sort (i.e. parents last) ConditionalGaussian::shared_ptr - x(new ConditionalGaussian(d1,R11,"y",S12)), - y(new ConditionalGaussian(d2,R22)); + Px_y(new ConditionalGaussian("x",d1,R11,"y",S12)), + Py(new ConditionalGaussian("y",d2,R22)); GaussianBayesNet cbn; - cbn.insert("x",x); - cbn.insert("y",y); + cbn.insert(Px_y); + cbn.insert(Py); return cbn; } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 62d67c76b..41a0bbb43 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -4,31 +4,33 @@ * @author Frank Dellaert */ -#include // for 'insert()' #include // for operator += using namespace boost::assign; #include #include "SymbolicBayesNet.h" +#include "GaussianBayesNet.h" +#include "Ordering.h" #include "BayesTree-inl.h" -#include "SmallExample.h" +#include "smallExample.h" using namespace gtsam; // Conditionals for ASIA example from the tutorial with A and D evidence -SymbolicConditional::shared_ptr B(new SymbolicConditional()), L( - new SymbolicConditional("B")), E(new SymbolicConditional("L", "B")), S( - new SymbolicConditional("L", "B")), T(new SymbolicConditional("E", "L")), - X(new SymbolicConditional("E")); +SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( + new SymbolicConditional("L", "B")), E( + new SymbolicConditional("E", "L", "B")), S(new SymbolicConditional("S", + "L", "B")), T(new SymbolicConditional("T", "E", "L")), X( + new SymbolicConditional("X", "E")); /* ************************************************************************* */ TEST( BayesTree, Front ) { - Front f1("B", B); - f1.add("L", L); - Front f2("L", L); - f2.add("B", B); + Front f1(B); + f1.add(L); + Front f2(L); + f2.add(B); CHECK(f1.equals(f1)); CHECK(!f1.equals(f2)); } @@ -38,31 +40,31 @@ TEST( BayesTree, constructor ) { // Create using insert BayesTree bayesTree; - bayesTree.insert("B", B); - bayesTree.insert("L", L); - bayesTree.insert("E", E); - bayesTree.insert("S", S); - bayesTree.insert("T", T); - bayesTree.insert("X", X); + bayesTree.insert(B); + bayesTree.insert(L); + bayesTree.insert(E); + bayesTree.insert(S); + bayesTree.insert(T); + bayesTree.insert(X); // Check Size LONGS_EQUAL(4,bayesTree.size()); // Check root - Front expected_root("B", B); - expected_root.add("L", L); - expected_root.add("E", E); + Front expected_root(B); + expected_root.add(L); + expected_root.add(E); Front actual_root = bayesTree.root(); CHECK(assert_equal(expected_root,actual_root)); // Create from symbolic Bayes chain in which we want to discover cliques SymbolicBayesNet ASIA; - ASIA.insert("X", X); - ASIA.insert("T", T); - ASIA.insert("S", S); - ASIA.insert("E", E); - ASIA.insert("L", L); - ASIA.insert("B", B); + ASIA.insert(X); + ASIA.insert(T); + ASIA.insert(S); + ASIA.insert(E); + ASIA.insert(L); + ASIA.insert(B); BayesTree bayesTree2(ASIA); // Check whether the same diff --git a/cpp/testConditionalGaussian.cpp b/cpp/testConditionalGaussian.cpp index 6e0833d58..d14c9fcbf 100644 --- a/cpp/testConditionalGaussian.cpp +++ b/cpp/testConditionalGaussian.cpp @@ -17,7 +17,6 @@ #include "Matrix.h" #include "ConditionalGaussian.h" - using namespace gtsam; /* ************************************************************************* */ @@ -42,8 +41,8 @@ TEST( ConditionalGaussian, equals ) d(0) = 0.2; d(1) = 0.5; ConditionalGaussian - expected(d, R, "x1", A1, "l1", A2), - actual(d, R, "x1", A1, "l1", A2); + expected("x",d, R, "x1", A1, "l1", A2), + actual("x",d, R, "x1", A1, "l1", A2); CHECK( expected.equals(actual) ); @@ -72,7 +71,7 @@ TEST( ConditionalGaussian, solve ) Vector d(2); d(0) = 0.2; d(1) = 0.5; - ConditionalGaussian cg(d, R, "x1", A1, "l1", A2); + ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2); Vector sx1(2); sx1(0) = 0.2; sx1(1) = 0.5; @@ -96,39 +95,38 @@ TEST( ConditionalGaussian, solve ) #ifdef HAVE_BOOST_SERIALIZATION TEST( ConditionalGaussian, serialize ) { -// // create a conditional gaussion node -// Matrix A1(2,2); -// A1(0,0) = 1 ; A1(1,0) = 2; -// A1(0,1) = 3 ; A1(1,1) = 4; -// -// Matrix A2(2,2); -// A2(0,0) = 6 ; A2(1,0) = 0.2; -// A2(0,1) = 8 ; A2(1,1) = 0.4; -// -// Matrix R(2,2); -// R(0,0) = 0.1 ; R(1,0) = 0.3; -// R(0,1) = 0.0 ; R(1,1) = 0.34; -// -// Vector d(2); -// d(0) = 0.2; d(1) = 0.5; -// -// ConditionalGaussian cg(d, R, "x1", A1, "l1", A2); -// -// //serialize the CG -// std::ostringstream in_archive_stream; -// boost::archive::text_oarchive in_archive(in_archive_stream); -// in_archive << cg; -// std::string serialized = in_archive_stream.str(); -// -// //deserialize the CGg -// std::istringstream out_archive_stream(serialized); -// boost::archive::text_iarchive out_archive(out_archive_stream); -// ConditionalGaussian output; -// out_archive >> output; -// -// //check for equality -// CHECK(cg.equals(output)); + // create a conditional gaussion node + Matrix A1(2,2); + A1(0,0) = 1 ; A1(1,0) = 2; + A1(0,1) = 3 ; A1(1,1) = 4; + Matrix A2(2,2); + A2(0,0) = 6 ; A2(1,0) = 0.2; + A2(0,1) = 8 ; A2(1,1) = 0.4; + + Matrix R(2,2); + R(0,0) = 0.1 ; R(1,0) = 0.3; + R(0,1) = 0.0 ; R(1,1) = 0.34; + + Vector d(2); + d(0) = 0.2; d(1) = 0.5; + + ConditionalGaussian cg("x2", d, R, "x1", A1, "l1", A2); + + //serialize the CG + std::ostringstream in_archive_stream; + boost::archive::text_oarchive in_archive(in_archive_stream); + in_archive << cg; + std::string serialized = in_archive_stream.str(); + + //deserialize the CGg + std::istringstream out_archive_stream(serialized); + boost::archive::text_iarchive out_archive(out_archive_stream); + ConditionalGaussian output; + out_archive >> output; + + //check for equality + CHECK(cg.equals(output)); } #endif //HAVE_BOOST_SERIALIZATION /* ************************************************************************* */ diff --git a/cpp/testConstrainedConditionalGaussian.cpp b/cpp/testConstrainedConditionalGaussian.cpp index 6e6dfdb4a..4396d708a 100644 --- a/cpp/testConstrainedConditionalGaussian.cpp +++ b/cpp/testConstrainedConditionalGaussian.cpp @@ -17,7 +17,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary1 ) // check unary constructor that doesn't require an R matrix // assumed identity matrix - ConstrainedConditionalGaussian unary(v); + ConstrainedConditionalGaussian unary("x1",v); VectorConfig fg; fg.insert("x1", v); @@ -32,7 +32,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 ) // check unary constructor that makes use of a A matrix Matrix A = eye(2) * 10; - ConstrainedConditionalGaussian unary(10*v, A); + ConstrainedConditionalGaussian unary("x1",10*v, A); VectorConfig fg; fg.insert("x1", v); @@ -51,7 +51,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 ) A(1,0) = 2.0 ; A(1,1) = 1.0; Vector rhs = A*v; - ConstrainedConditionalGaussian unary(rhs, A); + ConstrainedConditionalGaussian unary("x1",rhs, A); VectorConfig fg; fg.insert("x1", v); @@ -84,7 +84,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 ) Vector expected = Vector_(2, -3.3333, 0.6667); - ConstrainedConditionalGaussian binary(b, A1, "x1", A2); + ConstrainedConditionalGaussian binary("x2",b, A1, "x1", A2); CHECK(assert_equal(expected, binary.solve(fg), 1e-4)); } @@ -119,7 +119,7 @@ TEST (ConstrainedConditionalGaussian, basic_ternary1 ) Vector expected = Vector_(2, 6.6667, -9.3333); - ConstrainedConditionalGaussian ternary(b, A1, "x1", A2, "x2", A3); + ConstrainedConditionalGaussian ternary("x3",b, A1, "x1", A2, "x2", A3); CHECK(assert_equal(expected, ternary.solve(fg), 1e-4)); } diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index 6d6b40a8c..a84201b28 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -7,6 +7,7 @@ #include #include "ConstrainedLinearFactorGraph.h" #include "LinearFactorGraph.h" +#include "Ordering.h" #include "smallExample.h" using namespace gtsam; @@ -40,7 +41,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; Vector b2 = Vector_(2, 1.0, 2.0); - ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1); + ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1); CHECK(expectedCCG1.equals(*((*cbn)["x"]))); // verify remaining factor on y @@ -64,7 +65,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) R(0, 0) = 74.5356; R(0, 1) = -59.6285; R(1, 0) = 0.0; R(1, 1) = 44.7214; Vector br = Vector_(2, 8.9443, 4.4721); - ConditionalGaussian expected2(br, R); + ConditionalGaussian expected2("y",br, R); CHECK(expected2.equals(*((*cbn)["y"]))); } @@ -236,18 +237,18 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint ) // eliminate the constraint ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x"); - CHECK(cg1->size() == 1); + CHECK(cg1->nrParents() == 1); CHECK(fg.nrFactors() == 1); // eliminate the induced constraint ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y"); - CHECK(cg2->size() == 1); + CHECK(cg2->nrParents() == 1); CHECK(fg.nrFactors() == 0); // eliminate the linear factor ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne("z"); + CHECK(cg3->nrParents() == 0); CHECK(fg.size() == 0); - CHECK(cg3->size() == 0); // solve piecewise VectorConfig actual; diff --git a/cpp/testGaussianBayesNet.cpp b/cpp/testGaussianBayesNet.cpp index 5f82ca32d..00d88aecb 100644 --- a/cpp/testGaussianBayesNet.cpp +++ b/cpp/testGaussianBayesNet.cpp @@ -20,6 +20,7 @@ #include "GaussianBayesNet.h" #include "smallExample.h" +using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -35,7 +36,7 @@ TEST( GaussianBayesNet, constructor ) d1(0) = 9; d2(0) = 5; // define nodes and specify in reverse topological sort (i.e. parents last) - ConditionalGaussian x(d1,R11,"y",S12), y(d2,R22); + ConditionalGaussian x("x",d1,R11,"y",S12), y("y",d2,R22); // check small example which uses constructor GaussianBayesNet cbn = createSmallGaussianBayesNet(); @@ -82,44 +83,40 @@ TEST( GaussianBayesNet, optimize ) #ifdef HAVE_BOOST_SERIALIZATION TEST( GaussianBayesNet, serialize ) { -// //create a starting CBN -// GaussianBayesNet cbn = createSmallGaussianBayesNet(); -// -// //serialize the CBN -// std::ostringstream in_archive_stream; -// boost::archive::text_oarchive in_archive(in_archive_stream); -// in_archive << cbn; -// std::string serialized = in_archive_stream.str(); -// -// //DEBUG -// std::cout << "CBN Raw string: [" << serialized << "]" << std::endl; -// -// //remove newlines/carriage returns -// std::string clean; -// BOOST_FOREACH(char s, serialized) -// { -// if (s != '\n') -// { -// //copy in character -// clean.append(std::string(1,s)); -// } -// else -// { -// std::cout << " Newline character found!" << std::endl; -// //replace with an identifiable string -// clean.append(std::string(1,' ')); -// } -// } -// -// -// std::cout << "Cleaned CBN String: [" << clean << "]" << std::endl; -// -// //deserialize the CBN -// std::istringstream out_archive_stream(clean); -// boost::archive::text_iarchive out_archive(out_archive_stream); -// GaussianBayesNet output; -// out_archive >> output; -// CHECK(cbn.equals(output)); + //create a starting CBN + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + + //serialize the CBN + ostringstream in_archive_stream; + boost::archive::text_oarchive in_archive(in_archive_stream); + in_archive << cbn; + string serialized = in_archive_stream.str(); + + //DEBUG + cout << "CBN Raw string: [" << serialized << "]" << endl; + + //remove newlines/carriage returns + string clean; + BOOST_FOREACH(char s, serialized) { + if (s != '\n') { + //copy in character + clean.append(string(1,s)); + } + else { + cout << " Newline character found!" << endl; + //replace with an identifiable string + clean.append(string(1,' ')); + } + } + + cout << "Cleaned CBN String: [" << clean << "]" << endl; + + //deserialize the CBN + istringstream out_archive_stream(clean); + boost::archive::text_iarchive out_archive(out_archive_stream); + GaussianBayesNet output; + out_archive >> output; + CHECK(cbn.equals(output)); } #endif //HAVE_BOOST_SERIALIZATION diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index fae023c84..1e355e70e 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -14,6 +14,8 @@ using namespace boost::assign; #include #include "Matrix.h" +#include "Ordering.h" +#include "ConditionalGaussian.h" #include "smallExample.h" using namespace std; @@ -314,7 +316,7 @@ TEST( LinearFactor, eliminate ) +0.00,-8.94427 ); Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - ConditionalGaussian expectedCG(d,R11,"l1",S12,"x1",S13); + ConditionalGaussian expectedCG("x2",d,R11,"l1",S12,"x1",S13); // the expected linear factor Matrix Bl1 = Matrix_(2,2, @@ -384,7 +386,7 @@ TEST( LinearFactor, eliminate2 ) +0.00,-2.23607,+0.00,-8.94427 ); Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - ConditionalGaussian expectedCG(d,R11,"l1x1",S12); + ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12); // the expected linear factor Matrix Bl1x1 = Matrix_(2,4, @@ -424,7 +426,7 @@ TEST( LinearFactor, eliminate_empty ) boost::tie(actualCG,actualLF) = f.eliminate("x2"); // expected Conditional Gaussian is just a parent-less node with P(x)=1 - ConditionalGaussian expectedCG; + ConditionalGaussian expectedCG("x2"); // expected remaining factor is still empty :-) LinearFactor expectedLF; @@ -495,8 +497,8 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) +0.00,-2.23607 ); Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - ConditionalGaussian::shared_ptr CG(new ConditionalGaussian(d,R11,"l1x1",S12) ); - LinearFactor actualLF("x2",CG); + ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12) ); + LinearFactor actualLF(CG); // actualLF.print(); LinearFactor expectedLF("x2",R11,"l1x1",S12,d); diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 0c8d120f8..043487e53 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -13,8 +13,11 @@ using namespace std; using namespace boost::assign; #include + #include "Matrix.h" +#include "Ordering.h" #include "smallExample.h" +#include "GaussianBayesNet.h" using namespace gtsam; @@ -182,7 +185,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 ) +0.00,-6.66667 ); Vector d(2); d(0) = -2; d(1) = -1.0/3.0; - ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13); + ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13); CHECK(assert_equal(expected,*actual,tol)); } @@ -209,7 +212,7 @@ TEST( LinearFactorGraph, eliminateOne_x2 ) +0.00,-8.94427 ); Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - ConditionalGaussian expected(d,R11,"l1",S12,"x1",S13); + ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13); CHECK(assert_equal(expected,*actual,tol)); } @@ -235,7 +238,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 ) +0.00,-3.53553 ); Vector d(2); d(0) = -0.707107; d(1) = 1.76777; - ConditionalGaussian expected(d,R11,"x1",S12,"x2",S13); + ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13); CHECK(assert_equal(expected,*actual,tol)); } @@ -248,7 +251,7 @@ TEST( LinearFactorGraph, eliminateAll ) 0.0, 10}; Matrix R1 = Matrix_(2,2, data1); Vector d1(2); d1(0) = -1; d1(1) = -1; - ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian(d1, R1)); + ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1)); double data21[] = { 6.7082, 0.0, 0.0, 6.7082}; @@ -257,7 +260,7 @@ TEST( LinearFactorGraph, eliminateAll ) 0.0, -6.7082}; Matrix A1 = Matrix_(2,2, data22); Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164; - ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian(d2, R2, "x1", A1)); + ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2, "x1", A1)); double data31[] = { 11.1803, 0.0, 0.0, 11.1803}; @@ -270,12 +273,12 @@ TEST( LinearFactorGraph, eliminateAll ) Matrix A22 = Matrix_(2,2, data33); Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525; - ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22)); + ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3, "l1", A21, "x1", A22)); GaussianBayesNet expected; - expected.insert("x2", cg3); - expected.insert("l1", cg2); - expected.insert("x1", cg1); + expected.insert(cg3); + expected.insert(cg2); + expected.insert(cg1); // Check one ordering LinearFactorGraph fg1 = createLinearFactorGraph(); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 66c965de0..733cafaba 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -13,6 +13,7 @@ using namespace boost::assign; #include #include "Matrix.h" +#include "Ordering.h" #include "smallExample.h" // template definitions diff --git a/cpp/testSymbolicBayesNet.cpp b/cpp/testSymbolicBayesNet.cpp index 8002b5f7a..8db84e20a 100644 --- a/cpp/testSymbolicBayesNet.cpp +++ b/cpp/testSymbolicBayesNet.cpp @@ -10,6 +10,7 @@ using namespace boost::assign; #include +#include "Ordering.h" #include "smallExample.h" #include "SymbolicBayesNet.h" #include "SymbolicFactorGraph.h" @@ -22,13 +23,13 @@ TEST( SymbolicBayesNet, constructor ) { // Create manually SymbolicConditional::shared_ptr - x2(new SymbolicConditional("l1", "x1")), - l1(new SymbolicConditional("x1")), - x1(new SymbolicConditional()); + x2(new SymbolicConditional("x2","l1", "x1")), + l1(new SymbolicConditional("l1","x1")), + x1(new SymbolicConditional("x1")); SymbolicBayesNet expected; - expected.insert("x2",x2); - expected.insert("l1",l1); - expected.insert("x1",x1); + expected.insert(x2); + expected.insert(l1); + expected.insert(x1); // Create from a factor graph LinearFactorGraph factorGraph = createLinearFactorGraph(); diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 89420b025..0a451e0d0 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -9,6 +9,7 @@ using namespace boost::assign; #include +#include "Ordering.h" #include "smallExample.h" #include "SymbolicFactorGraph.h" #include "SymbolicBayesNet.h" @@ -114,7 +115,7 @@ TEST( LinearFactorGraph, eliminateOne ) fg.eliminateOne("x1"); // create expected symbolic Conditional - SymbolicConditional expected("l1","x2"); + SymbolicConditional expected("x1","l1","x2"); CHECK(assert_equal(expected,*actual)); } @@ -123,14 +124,14 @@ TEST( LinearFactorGraph, eliminateOne ) TEST( LinearFactorGraph, eliminate ) { // create expected Chordal bayes Net - SymbolicConditional::shared_ptr x2(new SymbolicConditional("l1", "x1")); - SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1")); - SymbolicConditional::shared_ptr x1(new SymbolicConditional()); + SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1")); + SymbolicConditional::shared_ptr l1(new SymbolicConditional("l1", "x1")); + SymbolicConditional::shared_ptr x1(new SymbolicConditional("x1")); SymbolicBayesNet expected; - expected.insert("x2", x2); - expected.insert("l1", l1); - expected.insert("x1", x1); + expected.insert(x2); + expected.insert(l1); + expected.insert(x1); // create a test graph LinearFactorGraph factorGraph = createLinearFactorGraph();