From 34e913e236c5d713c154e3bb775837be62e7ea20 Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Wed, 9 Dec 2009 20:49:04 +0000
Subject: [PATCH] optimization over cliques started

---
 cpp/GaussianBayesTree.h | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/cpp/GaussianBayesTree.h b/cpp/GaussianBayesTree.h
index 8ac507dca..642ed0eb0 100644
--- a/cpp/GaussianBayesTree.h
+++ b/cpp/GaussianBayesTree.h
@@ -17,10 +17,28 @@
 
 #include "Testable.h"
 #include "BayesTree.h"
+#include "VectorConfig.h"
 #include "GaussianConditional.h"
 
 namespace gtsam {
 
 	typedef BayesTree<GaussianConditional> GaussianBayesTree;
+#if 0
+	// recursively optimize starting with this conditional and all children
+	void optimize(sharedClique clique, VectorConfig& result) {
+	  // parents are assumed to already be solved and available in result
+		BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, clique) {
+	    Vector x = cg->solve(result); // Solve for that variable
+	    result.insert(cg->key(),x);   // store result in partial solution
+	  }
+		BOOST_FOREACH(sharedClique child, clique->children_) {
+			optimize(child, result);
+		}
+	}
 
+	void optimize(const GaussianBayesTree& bayesTree, VectorConfig& result) {
+		// starting from the root, call optimize on each conditional
+		optimize(bayesTree.root(), result);
+	}
+#endif
 } /// namespace gtsam