Clique marginal and dramatically simplified single variable marginal.

cpp/BayesTree-inl.h

@@ -7,6 +7,7 @@
 #include <boost/foreach.hpp>
 #include "BayesTree.h"
 #include "FactorGraph-inl.h"
+#include "BayesNet-inl.h"
 
 namespace gtsam {
 
@@ -19,6 +20,14 @@ namespace gtsam {
 			this->push_back(conditional);
 		}
 
+	/* ************************************************************************* */
+	template<class Conditional>
+	Ordering BayesTree<Conditional>::Clique::keys() const {
+		Ordering frontal_keys = this->ordering(), keys = separator_;
+		keys.splice(keys.begin(),frontal_keys);
+		return keys;
+	}
+
 	/* ************************************************************************* */
 	template<class Conditional>
 	void BayesTree<Conditional>::Clique::print(const string& s) const {
@@ -41,30 +50,27 @@ namespace gtsam {
 				child->printTree(indent+"  ");
 		}
 
+	/* ************************************************************************* */
+	// The shortcut density is a conditional P(S|R) of the separator of this
+	// clique on the root. We can compute it recursively from the parent shortcut
+	// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
+	// TODO, why do we actually return a shared pointer, why does eliminate?
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
 	typename BayesTree<Conditional>::sharedBayesNet
 	BayesTree<Conditional>::Clique::shortcut(shared_ptr R) {
-		// The shortcut density is a conditional P(S|R) of the separator of this
-		// clique on the root. We can compute it recursively from the parent shortcut
-		// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
-
 		// A first base case is when this clique or its parent is the root,
 		// in which case we return an empty Bayes net.
-		if (R.get()==this || parent_==R) {
+		if (R.get()==this || parent_==R)
-			sharedBayesNet empty(new BayesNet<Conditional>);
+			return sharedBayesNet(new BayesNet<Conditional>);
-			return empty;
-		}
 
 		// The parent clique has a Conditional for each frontal node in Fp
 		// so we can obtain P(Fp|Sp) in factor graph form
 		FactorGraph<Factor> p_Fp_Sp(*parent_);
-		//p_Fp_Sp.print("p_Fp_Sp");
 
 		// If not the base case, obtain the parent shortcut P(Sp|R) as factors
 		FactorGraph<Factor> p_Sp_R(*parent_->shortcut<Factor>(R));
-		//p_Sp_R.print("p_Sp_R");
 
 		// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
 		FactorGraph<Factor> p_Cp_R = combine(p_Fp_Sp, p_Sp_R);
@@ -76,12 +82,7 @@ namespace gtsam {
 		// Keys corresponding to the root should not be added to the ordering at all.
 
 		// Get the key list Cp=Fp+Sp, which will form the basis for the integrands
-		Ordering integrands;
+		Ordering integrands = parent_->keys();
-		{
-		Ordering Fp = parent_->ordering(), Sp = parent_->separator_;
-		integrands.splice(integrands.end(),Fp);
-		integrands.splice(integrands.end(),Sp);
-		}
 
 		// Start ordering with the separator
 		Ordering ordering = separator_;
@@ -108,6 +109,29 @@ namespace gtsam {
 		return p_S_R;
 	}
 
+	/* ************************************************************************* */
+	// P(C) = \int_R P(F|S) P(S|R) P(R)
+	// TODO: Maybe we should integrate given parent marginal P(Cp),
+	// \int(Cp\S) P(F|S)P(S|Cp)P(Cp)
+	// Because the root clique could be very big.
+	/* ************************************************************************* */
+	template<class Conditional>
+	template<class Factor>
+	BayesNet<Conditional>
+	BayesTree<Conditional>::Clique::marginal(shared_ptr R) {
+		// If we are the root, just return this root
+		if (R.get()==this) return *R;
+
+		// Combine P(F|S), P(S|R), and P(R)
+		sharedBayesNet p_FSR = this->shortcut<Factor>(R);
+		p_FSR->push_front(*this);
+		p_FSR->push_back(*R);
+
+		// Find marginal on the keys we are interested in
+		BayesNet<Conditional> marginal = marginals<Factor>(*p_FSR,keys());
+		return marginal;
+	}
+
 	/* ************************************************************************* */
 	template<class Conditional>
 	BayesTree<Conditional>::BayesTree() {
@@ -167,50 +191,23 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	// Desired: recursive, memoizing version
+	// First finds clique marginal then marginalizes that
-	// Once we know the clique, can we do all with Nodes ?
-	// Sure, as P(x) = \int P(C|root)
-	// The natural cache is P(C|root), memoized, of course, in the clique C
-	// When any marginal is asked for, we calculate P(C|root) = P(C|Pi)P(Pi|root)
-	// Super-naturally recursive !!!!!
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
-	typename BayesTree<Conditional>::sharedConditional
+	BayesNet<Conditional>
 	BayesTree<Conditional>::marginal(const string& key) const {
 
-		// get clique containing key, and remove all factors below key
+		// get clique containing key
 		sharedClique clique = (*this)[key];
-		Ordering ordering = clique->ordering();
-		FactorGraph<Factor> graph(*clique);
-		while(ordering.front()!=key) {
-			graph.findAndRemoveFactors(ordering.front());
-			ordering.pop_front();
-		}
 
-		// find all cliques on the path to the root and turn into factor graph
+		// calculate or retrieve its marginal
-		while (clique->parent_!=NULL) {
+		BayesNet<Conditional> cliqueMarginal = clique->marginal<Factor>(root_);
-			// move up the tree
-			clique = clique->parent_;
 
-			// extend ordering
+		// Get the marginal on the single key
-			Ordering cliqueOrdering = clique->ordering();
+		BayesNet<Conditional> marginal = marginals<Factor>(cliqueMarginal,Ordering(key));
-			ordering.splice (ordering.end(), cliqueOrdering);
 
-			// extend factor graph
+		return marginal;
-			FactorGraph<Factor> cliqueGraph(*clique);
-			typename FactorGraph<Factor>::const_iterator factor=cliqueGraph.begin();
-			for(; factor!=cliqueGraph.end(); factor++)
-				graph.push_back(*factor);
-		}
-
-		// TODO: can we prove reverse ordering is efficient?
-		ordering.reverse();
-
-		// eliminate to get marginal
-		sharedBayesNet chordalBayesNet = _eliminate<Factor,Conditional>(graph,ordering);
-
-		return chordalBayesNet->back(); // the root is the marginal
 	}
 
 	/* ************************************************************************* */

cpp/BayesTree.h

@@ -46,6 +46,9 @@ namespace gtsam {
 			//* Constructor */
 			Clique(const sharedConditional& conditional);
 
+			/** return keys in frontal:separator order */
+			Ordering keys() const;
+
 			/** print this node */
 			void print(const std::string& s = "Bayes tree node") const;
 
@@ -62,7 +65,11 @@ namespace gtsam {
 
 			/** return the conditional P(S|Root) on the separator given the root */
 			template<class Factor>
-			sharedBayesNet shortcut(shared_ptr R);
+			sharedBayesNet shortcut(shared_ptr root);
+
+			/** return the marginal P(C) of the clique */
+			template<class Factor>
+			BayesNet<Conditional> marginal(shared_ptr root);
 		};
 
 		typedef boost::shared_ptr<Clique> sharedClique;
@@ -130,7 +137,7 @@ namespace gtsam {
 
 		/** return marginal on any variable */
 		template<class Factor>
-		sharedConditional marginal(const std::string& key) const;
+		BayesNet<Conditional> marginal(const std::string& key) const;
 	}; // BayesTree
 
 } /// namespace gtsam

cpp/testBayesTree.cpp

@@ -176,26 +176,20 @@ TEST( BayesTree, balanced_smoother_marginals )
 
 	// Marginals
 
-	// Marginal will always be axis-parallel Gaussian on delta=(0,0)
-	Matrix R = eye(2);
-
 	// Check marginal on x1
-  Vector sigma1 = repeat(2, 0.786153);
+  GaussianBayesNet expected1("x1", delta, 0.786153);
-	ConditionalGaussian expected1("x1", delta, R, sigma1);
+	BayesNet<ConditionalGaussian>  actual1 = bayesTree.marginal<LinearFactor>("x1");
-	ConditionalGaussian::shared_ptr actual1 = bayesTree.marginal<LinearFactor>("x1");
+	CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected1,actual1,1e-4));
-	CHECK(assert_equal(expected1,*actual1,1e-4));
 
 	// Check marginal on x2
-  Vector sigma2 = repeat(2, 0.687131);
+  GaussianBayesNet expected2("x2", delta, 0.687131);
-	ConditionalGaussian expected2("x2", delta, R, sigma2);
+	BayesNet<ConditionalGaussian>  actual2 = bayesTree.marginal<LinearFactor>("x2");
-	ConditionalGaussian::shared_ptr actual2 = bayesTree.marginal<LinearFactor>("x2");
+	CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected2,actual2,1e-4));
-	CHECK(assert_equal(expected2,*actual2,1e-4));
 
 	// Check marginal on x3
-  Vector sigma3 = repeat(2, 0.671512);
+  GaussianBayesNet expected3("x3", delta, 0.671512);
-	ConditionalGaussian expected3("x3", delta, R, sigma3);
+	BayesNet<ConditionalGaussian>  actual3 = bayesTree.marginal<LinearFactor>("x3");
-	ConditionalGaussian::shared_ptr actual3 = bayesTree.marginal<LinearFactor>("x3");
+	CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected3,actual3,1e-4));
-	CHECK(assert_equal(expected3,*actual3,1e-4));
 }
 
 /* ************************************************************************* */
@@ -212,10 +206,10 @@ TEST( BayesTree, balanced_smoother_shortcuts )
 
 	// Create the Bayes tree
 	Gaussian bayesTree(*chordalBayesNet);
+	Gaussian::sharedClique R = bayesTree.root();
 
 	// Check the conditional P(Root|Root)
 	BayesNet<ConditionalGaussian> empty;
-	Gaussian::sharedClique R = bayesTree.root();
 	Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R);
 	CHECK(assert_equal(empty,*actual1,1e-4));
 
@@ -232,6 +226,33 @@ TEST( BayesTree, balanced_smoother_shortcuts )
 	CHECK(assert_equal(expected3,*actual3,1e-4));
 }
 
+/* ************************************************************************* */
+TEST( BayesTree, balanced_smoother_clique_marginals )
+{
+	// Create smoother with 7 nodes
+	LinearFactorGraph smoother = createSmoother(7);
+	Ordering ordering;
+	ordering += "x1","x3","x5","x7","x2","x6","x4";
+
+	// eliminate using a "nested dissection" ordering
+	GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering);
+	boost::shared_ptr<VectorConfig> actualSolution = chordalBayesNet->optimize();
+
+	// Create the Bayes tree
+	Gaussian bayesTree(*chordalBayesNet);
+	Gaussian::sharedClique R = bayesTree.root();
+
+	// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
+	GaussianBayesNet expected3("x2",zero(2),0.687131);
+  Vector sigma3 = repeat(2, 0.707107);
+  Matrix A12 = (-0.5)*eye(2);
+	ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma3));
+	expected3.push_front(cg3);
+	Gaussian::sharedClique C3 = bayesTree["x1"];
+	BayesNet<ConditionalGaussian> actual3 = C3->marginal<LinearFactor>(R);
+	CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected3,actual3,1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;