Working recursive (but not cached) shortcut.

cpp/BayesTree-inl.h

@@ -43,20 +43,68 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Conditional>
+	template<class Factor>
 	typename BayesTree<Conditional>::sharedBayesNet
-	BayesTree<Conditional>::Clique::shortcut() {
+	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(S_p|R) as \int P(F_p|S_p) P(S_p|R), where F_p are the frontal nodes in p
+		// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
 
-		// The base case is when we are the root or the parent is the root,
+		// A first base case is when this clique or its parent is the root,
-		// in which case we return an empty Bayes net
+		// in which case we return an empty Bayes net.
-		sharedBayesNet p_S_R(new BayesNet<Conditional>);
+		if (R.get()==this || parent_==R) {
-		if (parent_==NULL || parent_->parent_==NULL) return p_S_R;
+			sharedBayesNet empty(new BayesNet<Conditional>);
+			return empty;
+		}
 
-		// If not the base case, calculate the parent shortcut P(S_p|R)
+		// The parent clique has a Conditional for each frontal node in Fp
-		sharedBayesNet p_Sp_R = parent_->shortcut();
+		// 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);
+
+		// Eliminate into a Bayes net with ordering designed to integrate out
+		// any variables not in *our* separator. Variables to integrate out must be
+		// eliminated first hence the desired ordering is [Cp\S S].
+		// However, an added wrinkle is that Cp might overlap with the root.
+		// 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 Fp = parent_->ordering(), Sp = parent_->separator_;
+		integrands.splice(integrands.end(),Fp);
+		integrands.splice(integrands.end(),Sp);
+		}
+
+		// Start ordering with the separator
+		Ordering ordering = separator_;
+
+		// remove any variables in the root, after this integrands = Cp\R, ordering = S\R
+		BOOST_FOREACH(string key, R->ordering()) {
+			integrands.remove(key);
+			ordering.remove(key);
+		}
+
+		// remove any variables in the separator, after this integrands = Cp\R\S
+		BOOST_FOREACH(string key, separator_) integrands.remove(key);
+
+		// form the ordering as [Cp\R\S S\R]
+		BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key);
+
+		// eliminate to get marginal
+		sharedBayesNet p_S_R = _eliminate<Factor,Conditional>(p_Cp_R,ordering);
+
+		// remove all integrands
+		BOOST_FOREACH(string key, integrands) p_S_R->pop_front();
+
+		// return the parent shortcut P(Sp|R)
 		return p_S_R;
 	}
 

cpp/BayesTree.h

@@ -38,7 +38,7 @@ namespace gtsam {
 		 */
 		struct Clique: public BayesNet<Conditional> {
 
-			typedef boost::shared_ptr<Clique> shared_ptr;
+			typedef typename boost::shared_ptr<Clique> shared_ptr;
 			shared_ptr parent_;
 			std::list<shared_ptr> children_;
 			std::list<std::string> separator_; /** separator keys */
@@ -46,19 +46,23 @@ namespace gtsam {
 			//* Constructor */
 			Clique(const sharedConditional& conditional);
 
+			/** print this node */
+			void print(const std::string& s = "Bayes tree node") const;
+
 			/** The size *includes* the separator */
 			size_t size() const {
 				return this->conditionals_.size() + separator_.size();
 			}
 
-			/** print this node */
+			/** is this the root of a Bayes tree ? */
-			void print(const std::string& s = "Bayes tree node") const;
+			inline bool isRoot() const { return parent_==NULL;}
 
 			/** print this node and entire subtree below it */
 			void printTree(const std::string& indent) const;
 
 			/** return the conditional P(S|Root) on the separator given the root */
-			sharedBayesNet shortcut();
+			template<class Factor>
+			sharedBayesNet shortcut(shared_ptr R);
 		};
 
 		typedef boost::shared_ptr<Clique> sharedClique;
@@ -119,7 +123,7 @@ namespace gtsam {
 		sharedClique operator[](const std::string& key) const {
 			typename Nodes::const_iterator it = nodes_.find(key);
 			if (it == nodes_.end()) throw(std::invalid_argument(
-					"BayesTree::operator['" + key + "'): key not found"));
+					"BayesTree::operator['" + key + "']: key not found"));
 			sharedClique clique = it->second;
 			return clique;
 		}

cpp/testBayesTree.cpp

@@ -101,21 +101,54 @@ TEST( BayesTree, smoother )
 	Gaussian bayesTree(*chordalBayesNet);
 	LONGS_EQUAL(7,bayesTree.size());
 
-	// Get the conditional P(S6|Root)
+	// Check the conditional P(Root|Root)
-  Vector sigma1 = repeat(2, 0.786153);
+	BayesNet<ConditionalGaussian> empty;
-	ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x2", zero(2), eye(2), sigma1));
+	Gaussian::sharedClique R = bayesTree.root();
-	BayesNet<ConditionalGaussian> expected; expected.push_back(cg);
+	Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R);
-	Gaussian::sharedClique C6 = bayesTree["x1"];
+	CHECK(assert_equal(empty,*actual1,1e-4));
-	Gaussian::sharedBayesNet actual = C6->shortcut();
+
-	//CHECK(assert_equal(expected,*actual,1e-4));
+	// Check the conditional P(C2|Root)
+	Gaussian::sharedClique C2 = bayesTree["x5"];
+	Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(R);
+	CHECK(assert_equal(empty,*actual2,1e-4));
+
+	// Check the conditional P(C3|Root)
+  Vector sigma3 = repeat(2, 0.61808);
+  Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
+	ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3));
+	BayesNet<ConditionalGaussian> expected3; expected3.push_back(cg3);
+	Gaussian::sharedClique C3 = bayesTree["x4"];
+	Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(R);
+	CHECK(assert_equal(expected3,*actual3,1e-4));
+
+	// Check the conditional P(C4|Root)
+  Vector sigma4 = repeat(2, 0.661968);
+  Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
+	ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4));
+	BayesNet<ConditionalGaussian> expected4; expected4.push_back(cg4);
+	Gaussian::sharedClique C4 = bayesTree["x3"];
+	Gaussian::sharedBayesNet actual4 = C4->shortcut<LinearFactor>(R);
+	CHECK(assert_equal(expected4,*actual4,1e-4));
 }
 
 /* ************************************************************************* *
  Bayes tree for smoother with "nested dissection" ordering:
- x5 x6 x4
+
-   x3 x2 : x4
+	 Node[x1] P(x1 | x2)
-     x1 : x2
+	 Node[x3] P(x3 | x2 x4)
-   x7 : x6
+	 Node[x5] P(x5 | x4 x6)
+	 Node[x7] P(x7 | x6)
+	 Node[x2] P(x2 | x4)
+	 Node[x6] P(x6 | x4)
+	 Node[x4] P(x4)
+
+ becomes
+
+	 C1		 x5 x6 x4
+	 C2		  x3 x2 : x4
+	 C3		    x1 : x2
+	 C4		  x7 : x6
+
 /* ************************************************************************* */
 TEST( BayesTree, balanced_smoother_marginals )
 {
@@ -126,27 +159,22 @@ TEST( BayesTree, balanced_smoother_marginals )
 
 	// eliminate using a "nested dissection" ordering
 	GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering);
-  boost::shared_ptr<VectorConfig> actualSolution = chordalBayesNet->optimize();
+
+//  SymbolicBayesNet symbolic(*chordalBayesNet);
+//  symbolic.print("chordalBayesNet");
 
   VectorConfig expectedSolution;
   Vector delta = zero(2);
-  expectedSolution.insert("x1",delta);
+  BOOST_FOREACH(string key, ordering)
-  expectedSolution.insert("x2",delta);
+		expectedSolution.insert(key,delta);
-  expectedSolution.insert("x3",delta);
+  boost::shared_ptr<VectorConfig> actualSolution = chordalBayesNet->optimize();
-  expectedSolution.insert("x4",delta);
-  expectedSolution.insert("x5",delta);
-  expectedSolution.insert("x6",delta);
-  expectedSolution.insert("x7",delta);
 	CHECK(assert_equal(expectedSolution,*actualSolution,1e-4));
 
 	// Create the Bayes tree
 	Gaussian bayesTree(*chordalBayesNet);
 	LONGS_EQUAL(7,bayesTree.size());
 
-	// Check root clique
+	// Marginals
-	//BayesNet<ConditionalGaussian> expected_root;
-	//BayesNet<ConditionalGaussian> actual_root = bayesTree.root();
-	//CHECK(assert_equal(expected_root,actual_root));
 
 	// Marginal will always be axis-parallel Gaussian on delta=(0,0)
 	Matrix R = eye(2);
@@ -170,6 +198,40 @@ TEST( BayesTree, balanced_smoother_marginals )
 	CHECK(assert_equal(expected3,*actual3,1e-4));
 }
 
+/* ************************************************************************* */
+TEST( BayesTree, balanced_smoother_shortcuts )
+{
+	// 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);
+
+	// 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));
+
+	// Check the conditional P(C2|Root)
+	Gaussian::sharedClique C2 = bayesTree["x3"];
+	Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(R);
+	CHECK(assert_equal(empty,*actual2,1e-4));
+
+	// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
+	ConditionalGaussian::shared_ptr p_x2_x4 = (*chordalBayesNet)["x2"];
+	BayesNet<ConditionalGaussian> expected3; expected3.push_back(p_x2_x4);
+	Gaussian::sharedClique C3 = bayesTree["x1"];
+	Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(R);
+	CHECK(assert_equal(expected3,*actual3,1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;