Added BayesNet::popLeaf

gtsam/inference/BayesNet-inl.h

@@ -49,6 +49,24 @@ namespace gtsam {
 		return equal(conditionals_.begin(),conditionals_.end(),cbn.conditionals_.begin(),equals_star<CONDITIONAL>(tol));
 	}
 
+  /* ************************************************************************* */
+	template<class CONDITIONAL>
+	typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::find(Index key) const {
+	  for(const_iterator it = begin(); it != end(); ++it)
+	    if(std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) != (*it)->endFrontals())
+	      return it;
+	  return end();
+	}
+
+  /* ************************************************************************* */
+  template<class CONDITIONAL>
+  typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::find(Index key) {
+    for(iterator it = begin(); it != end(); ++it)
+      if(std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) != (*it)->endFrontals())
+        return it;
+    return end();
+  }
+
   /* ************************************************************************* */
   template<class CONDITIONAL>
 	void BayesNet<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
@@ -82,6 +100,21 @@ namespace gtsam {
 			push_front(conditional);
 	}
 
+  /* ************************************************************************* */
+  template<class CONDITIONAL>
+	void BayesNet<CONDITIONAL>::popLeaf(iterator conditional) {
+#ifndef NDEBUG
+    BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) {
+      BOOST_FOREACH(Index key, (*conditional)->frontals()) {
+        if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents())
+          throw std::invalid_argument(
+              "Debug mode exception:  in BayesNet::popLeaf, the requested conditional is not a leaf.");
+      }
+    }
+#endif
+    conditionals_.erase(conditional);
+	}
+
 	/* ************************************************************************* */
 	template<class CONDITIONAL>
 	FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
@@ -118,7 +151,7 @@ namespace gtsam {
       }
 	  }
 		throw(invalid_argument((boost::format(
-						"BayesNet::operator['%1%']: not found") % key).str()));
+		    "BayesNet::operator['%1%']: not found") % key).str()));
 	}
 
 	/* ************************************************************************* */

gtsam/inference/BayesNet.h

@@ -22,6 +22,7 @@
 #include <list>
 #include <boost/shared_ptr.hpp>
 #include <boost/serialization/nvp.hpp>
+#include <boost/assign/list_inserter.hpp>
 
 #include <gtsam/base/types.h>
 #include <gtsam/base/FastList.h>
@@ -30,126 +31,181 @@
 
 namespace gtsam {
 
-	/**
+/**
-	 * A BayesNet is a list of conditionals, stored in elimination order, i.e.
+ * A BayesNet is a list of conditionals, stored in elimination order, i.e.
-	 * leaves first, parents last.  GaussianBayesNet and SymbolicBayesNet are
+ * leaves first, parents last.  GaussianBayesNet and SymbolicBayesNet are
-	 * defined as typedefs of this class, using GaussianConditional and
+ * defined as typedefs of this class, using GaussianConditional and
-	 * IndexConditional as the CONDITIONAL template argument.
+ * IndexConditional as the CONDITIONAL template argument.
-	 *
+ *
-	 * todo:  Symbolic using Index is a misnomer.
+ * todo:  Symbolic using Index is a misnomer.
-	 * todo:  how to handle Bayes nets with an optimize function?  Currently using global functions.
+ * todo:  how to handle Bayes nets with an optimize function?  Currently using global functions.
-	 */
+ */
-	template<class CONDITIONAL>
+template<class CONDITIONAL>
-	class BayesNet: public Testable<BayesNet<CONDITIONAL> > {
+class BayesNet: public Testable<BayesNet<CONDITIONAL> > {
 
-	public:
+public:
 
-	  typedef typename boost::shared_ptr<BayesNet<CONDITIONAL> > shared_ptr;
+  typedef typename boost::shared_ptr<BayesNet<CONDITIONAL> > shared_ptr;
 
-		/** We store shared pointers to Conditional densities */
+  /** We store shared pointers to Conditional densities */
-		typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
+  typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
-		typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
+  typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
-		typedef typename std::list<sharedConditional> Conditionals;
+  typedef typename std::list<sharedConditional> Conditionals;
 
-    typedef typename Conditionals::const_iterator iterator;
+  typedef typename Conditionals::iterator iterator;
-    typedef typename Conditionals::const_reverse_iterator reverse_iterator;
+  typedef typename Conditionals::reverse_iterator reverse_iterator;
-		typedef typename Conditionals::const_iterator const_iterator;
+  typedef typename Conditionals::const_iterator const_iterator;
-		typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
+  typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
 
-	protected:
+protected:
 
-		/**
+  /**
-		 *  Conditional densities are stored in reverse topological sort order (i.e., leaves first,
+   *  Conditional densities are stored in reverse topological sort order (i.e., leaves first,
-		 *  parents last), which corresponds to the elimination ordering if so obtained,
+   *  parents last), which corresponds to the elimination ordering if so obtained,
-		 *  and is consistent with the column (block) ordering of an upper triangular matrix.
+   *  and is consistent with the column (block) ordering of an upper triangular matrix.
-		 */
+   */
-		Conditionals conditionals_;
+  Conditionals conditionals_;
 
-	public:
+public:
 
-		/** Default constructor as an empty BayesNet */
+  /** Default constructor as an empty BayesNet */
-		BayesNet() {};
+  BayesNet() {};
 
-		/** BayesNet with 1 conditional */
+  /** BayesNet with 1 conditional */
-		BayesNet(const sharedConditional& conditional) { push_back(conditional); }
+  BayesNet(const sharedConditional& conditional) { push_back(conditional); }
 
-		/** print */
+  /** print */
-		void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const;
 
-		/** check equality */
+  /** check equality */
-		bool equals(const BayesNet& other, double tol = 1e-9) const;
+  bool equals(const BayesNet& other, double tol = 1e-9) const;
 
-		/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
+  /** Find an iterator pointing to the conditional where the specified key
-		inline void push_back(const sharedConditional& conditional) {
+   * appears as a frontal variable, or end() if no conditional contains this
-			conditionals_.push_back(conditional);
+   * key.  Running time is approximately \f$ O(n) \f$ in the number of
-		}
+   * conditionals in the BayesNet.
+   * @param key The index to find in the frontal variables of a conditional.
+   */
+  const_iterator find(Index key) const;
 
-		/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
+  /** Find an iterator pointing to the conditional where the specified key
-		inline void push_front(const sharedConditional& conditional) {
+   * appears as a frontal variable, or end() if no conditional contains this
-			conditionals_.push_front(conditional);
+   * key.  Running time is approximately \f$ O(n) \f$ in the number of
-		}
+   * conditionals in the BayesNet.
+   * @param key The index to find in the frontal variables of a conditional.
+   */
+  iterator find(Index key);
 
-		// push_back an entire Bayes net */
+  /** push_back: use reverse topological sort (i.e. parents last / elimination order) */
-		void push_back(const BayesNet<CONDITIONAL> bn);
+  inline void push_back(const sharedConditional& conditional) {
+    conditionals_.push_back(conditional);
+  }
 
-		// push_front an entire Bayes net */
+  /** push_front: use topological sort (i.e. parents first / reverse elimination order) */
-		void push_front(const BayesNet<CONDITIONAL> bn);
+  inline void push_front(const sharedConditional& conditional) {
+    conditionals_.push_front(conditional);
+  }
 
-		/**
+  /// push_back an entire Bayes net
-		 * pop_front: remove node at the bottom, used in marginalization
+  void push_back(const BayesNet<CONDITIONAL> bn);
-		 * For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
-		 */
-		inline void pop_front() {conditionals_.pop_front();}
 
-		/** Permute the variables in the BayesNet */
+  /// push_front an entire Bayes net
-		void permuteWithInverse(const Permutation& inversePermutation);
+  void push_front(const BayesNet<CONDITIONAL> bn);
 
-		/**
+  /** += syntax for push_back, e.g. bayesNet += c1, c2, c3
-		 * Permute the variables when only separator variables need to be permuted.
+   * @param conditional The conditional to add to the back of the BayesNet
-	   * Returns true if any reordered variables appeared in the separator and
+   */
-	   * false if not.
+  boost::assign::list_inserter<boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >, sharedConditional>
-	   */
+  operator+=(const sharedConditional& conditional) {
-		bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+    return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >(*this))(conditional); }
 
-		/** size is the number of nodes */
+  /**
-		inline size_t size() const {
+   * pop_front: remove node at the bottom, used in marginalization
-			return conditionals_.size();
+   * For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
-		}
+   */
+  void pop_front() {conditionals_.pop_front();}
 
-		/** return keys in reverse topological sort order, i.e., elimination order */
+  /**
-    FastList<Index> ordering() const;
+   * Remove any leaf conditional.  The conditional to remove is specified by
+   * iterator.  To find the iterator pointing to the conditional containing a
+   * particular key, use find(), which has \f$ O(n) \f$ complexity.  The
+   * popLeaf function by itself has \f$ O(1) \f$ complexity.
+   *
+   * If the program calling this function is
+   * compiled without NDEBUG defined, this function will check that the node
+   * is indeed a leaf, but otherwise will not check, because the check has
+   * \f$ O(n^2) \f$ complexity.
+   *
+   * Example 1:
+     \code
+     // Remove a leaf node with a known conditional
+     GaussianBayesNet gbn = ...
+     GaussianBayesNet::iterator leafConditional = ...
+     gbn.popLeaf(leafConditional);
+     \endcode
+   * Example 2:
+     \code
+     // Remove the leaf node containing variable index 14
+     GaussianBayesNet gbn = ...
+     gbn.popLeaf(gbn.find(14));
+     \endcode
+   * @param conditional The iterator pointing to the leaf conditional to remove
+   */
+  void popLeaf(iterator conditional);
 
-    /** SLOW O(n) random access to Conditional by key */
+  /** Permute the variables in the BayesNet */
-		sharedConditional operator[](Index key) const;
+  void permuteWithInverse(const Permutation& inversePermutation);
 
-    /** return last node in ordering */
+  /**
-    sharedConditional& front() { return conditionals_.front(); }
+   * Permute the variables when only separator variables need to be permuted.
+   * Returns true if any reordered variables appeared in the separator and
+   * false if not.
+   */
+  bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
 
-    /** return last node in ordering */
+  /** size is the number of nodes */
-    boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
+  size_t size() const {
+    return conditionals_.size();
+  }
 
-		/** return last node in ordering */
+  /** return keys in reverse topological sort order, i.e., elimination order */
-		sharedConditional& back() { return conditionals_.back(); }
+  FastList<Index> ordering() const;
 
-		/** return last node in ordering */
+  /** SLOW O(n) random access to Conditional by key */
-		boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
+  sharedConditional operator[](Index key) const;
 
-		/** return iterators. FD: breaks encapsulation? */
+  /** return last node in ordering */
-		inline const_iterator const begin() const {return conditionals_.begin();}
+  sharedConditional& front() { return conditionals_.front(); }
-		inline const_iterator const end()   const {return conditionals_.end();}
-		inline const_reverse_iterator const rbegin() const {return conditionals_.rbegin();}
-		inline const_reverse_iterator const rend()   const {return conditionals_.rend();}
 
-		/** saves the bayes to a text file in GraphViz format */
+  /** return last node in ordering */
-//		void saveGraph(const std::string& s) const;
+  boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
 
-	private:
+  /** return last node in ordering */
-		/** Serialization function */
+  sharedConditional& back() { return conditionals_.back(); }
-		friend class boost::serialization::access;
+
-		template<class ARCHIVE>
+  /** return last node in ordering */
-		void serialize(ARCHIVE & ar, const unsigned int version) {
+  boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
-			ar & BOOST_SERIALIZATION_NVP(conditionals_);
+
-		}
+  /** return iterators. FD: breaks encapsulation? */
-	}; // BayesNet
+  const_iterator begin()          const {return conditionals_.begin();}
+  const_iterator end()            const {return conditionals_.end();}
+  const_reverse_iterator rbegin() const {return conditionals_.rbegin();}
+  const_reverse_iterator rend()   const {return conditionals_.rend();}
+  iterator begin()          {return conditionals_.begin();}
+  iterator end()            {return conditionals_.end();}
+  reverse_iterator rbegin() {return conditionals_.rbegin();}
+  reverse_iterator rend()   {return conditionals_.rend();}
+
+  /** saves the bayes to a text file in GraphViz format */
+  //		void saveGraph(const std::string& s) const;
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(conditionals_);
+  }
+}; // BayesNet
 
 } /// namespace gtsam

gtsam/inference/tests/testSymbolicBayesNet.cpp

@@ -22,7 +22,6 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/inference/IndexConditional.h>
-#ifdef ALL
 #include <gtsam/inference/SymbolicFactorGraph.h>
 
 using namespace std;
@@ -32,6 +31,8 @@ static const Index _L_ = 0;
 static const Index _A_ = 1;
 static const Index _B_ = 2;
 static const Index _C_ = 3;
+static const Index _D_ = 4;
+static const Index _E_ = 5;
 
 IndexConditional::shared_ptr
 	B(new IndexConditional(_B_)),
@@ -100,8 +101,92 @@ TEST( SymbolicBayesNet, combine )
 
   CHECK(assert_equal(expected,p_ABC));
 }
+
+/* ************************************************************************* */
+TEST(SymbolicBayesNet, find) {
+  SymbolicBayesNet bn;
+  bn += IndexConditional::shared_ptr(new IndexConditional(_A_, _B_));
+  std::vector<Index> keys;
+  keys.push_back(_B_);
+  keys.push_back(_C_);
+  keys.push_back(_D_);
+  bn += IndexConditional::shared_ptr(new IndexConditional(keys,2));
+  bn += IndexConditional::shared_ptr(new IndexConditional(_D_));
+
+  SymbolicBayesNet::iterator expected = bn.begin();  ++ expected;
+  SymbolicBayesNet::iterator actual = bn.find(_C_);
+  EXPECT(assert_equal(**expected, **actual));
+}
+
+/* ************************************************************************* */
+TEST_UNSAFE(SymbolicBayesNet, popLeaf) {
+  IndexConditional::shared_ptr
+    A(new IndexConditional(_A_,_E_)),
+    B(new IndexConditional(_B_,_E_)),
+    C(new IndexConditional(_C_,_D_)),
+    D(new IndexConditional(_D_,_E_)),
+    E(new IndexConditional(_E_));
+
+  // BayesNet after popping A
+  SymbolicBayesNet expected1;
+  expected1 += B, C, D, E;
+
+  // BayesNet after popping C
+  SymbolicBayesNet expected2;
+  expected2 += A, B, D, E;
+
+  // BayesNet after popping C and D
+  SymbolicBayesNet expected3;
+  expected3 += A, B, E;
+
+  // BayesNet after popping C and A
+  SymbolicBayesNet expected4;
+  expected4 += B, D, E;
+
+
+  // BayesNet after popping A
+  SymbolicBayesNet actual1;
+  actual1 += A, B, C, D, E;
+  actual1.popLeaf(actual1.find(_A_));
+
+  // BayesNet after popping C
+  SymbolicBayesNet actual2;
+  actual2 += A, B, C, D, E;
+  actual2.popLeaf(actual2.find(_C_));
+
+  // BayesNet after popping C and D
+  SymbolicBayesNet actual3;
+  actual3 += A, B, C, D, E;
+  actual3.popLeaf(actual3.find(_C_));
+  actual3.popLeaf(actual3.find(_D_));
+
+  // BayesNet after popping C and A
+  SymbolicBayesNet actual4;
+  actual4 += A, B, C, D, E;
+  actual4.popLeaf(actual4.find(_C_));
+  actual4.popLeaf(actual4.find(_A_));
+
+  EXPECT(assert_equal(expected1, actual1));
+  EXPECT(assert_equal(expected2, actual2));
+  EXPECT(assert_equal(expected3, actual3));
+  EXPECT(assert_equal(expected4, actual4));
+
+  // Try to remove a non-leaf node
+#undef NDEBUG_SAVED
+#ifdef NDEBUG
+#define NDEBUG_SAVED
 #endif
 
+#undef NDEBUG
+  SymbolicBayesNet actual5;
+  actual5 += A, B, C, D, E;
+  CHECK_EXCEPTION(actual5.popLeaf(actual5.find(_D_)), std::invalid_argument);
+
+#ifdef NDEBUG_SAVED
+#define NDEBUG
+#endif
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;