From 72ba1bee2405986f9cd2372a3cb5ca1a91fdb316 Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Wed, 9 Dec 2009 22:02:50 +0000
Subject: [PATCH] separate cpp

---
 cpp/GaussianBayesTree.cpp     | 42 +++++++++++++++++++++++++++++++++++
 cpp/GaussianBayesTree.h       | 26 +++++++---------------
 cpp/Makefile.am               |  1 +
 cpp/testGaussianBayesTree.cpp |  5 +++++
 4 files changed, 56 insertions(+), 18 deletions(-)
 create mode 100644 cpp/GaussianBayesTree.cpp

diff --git a/cpp/GaussianBayesTree.cpp b/cpp/GaussianBayesTree.cpp
new file mode 100644
index 000000000..0db508d43
--- /dev/null
+++ b/cpp/GaussianBayesTree.cpp
@@ -0,0 +1,42 @@
+/**
+ * @file    GaussianBayesTree
+ * @brief   Bayes Tree is a tree of cliques of a Bayes Chain
+ * @author  Michael Kaess
+ */
+
+#include <boost/foreach.hpp>
+
+#include "GaussianBayesTree.h"
+#include "VectorConfig.h"
+
+using namespace std;
+using namespace gtsam;
+
+// Explicitly instantiate so we don't have to include everywhere
+#include "BayesTree-inl.h"
+template class BayesTree<GaussianConditional>;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void optimize(GaussianBayesTree::sharedClique clique, VectorConfig& result) {
+#if 0
+	// 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(GaussianBayesTree::sharedClique child, clique->children_) {
+		optimize(child, result);
+	}
+#endif
+}
+
+/* ************************************************************************* */
+VectorConfig optimize(GaussianBayesTree& bayesTree) {
+	VectorConfig result;
+	// starting from the root, call optimize on each conditional
+	optimize(bayesTree.root(), result);
+}
+
+} /// namespace gtsam
diff --git a/cpp/GaussianBayesTree.h b/cpp/GaussianBayesTree.h
index 5d3975f58..27c3db158 100644
--- a/cpp/GaussianBayesTree.h
+++ b/cpp/GaussianBayesTree.h
@@ -19,26 +19,16 @@
 #include "BayesTree.h"
 #include "VectorConfig.h"
 #include "GaussianConditional.h"
+#include "GaussianBayesTree.h"
 
 namespace gtsam {
 
 	typedef BayesTree<GaussianConditional> GaussianBayesTree;
-#if 0
-	// recursively optimize starting with this conditional and all children
-	void optimize(GaussianBayesTree::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(GaussianBayesTree::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
+	// recursively optimize this conditional and all subtrees
+	void optimize(GaussianBayesTree::sharedClique clique, VectorConfig& result);
+
+	// optimize the BayesTree, starting from the root
+	VectorConfig optimize(GaussianBayesTree& bayesTree);
+
+}/// namespace gtsam
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 38beeb0c1..8712a4204 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -65,6 +65,7 @@ headers += inference.h inference-inl.h
 headers += FactorGraph.h FactorGraph-inl.h
 headers += BayesNet.h BayesNet-inl.h
 headers += BayesTree.h BayesTree-inl.h GaussianBayesTree.h
+sources += GaussianBayesTree.cpp
 check_PROGRAMS += testFactorgraph testBayesTree testGaussianBayesTree testInference testOrdering
 testFactorgraph_SOURCES        = testFactorgraph.cpp
 testBayesTree_SOURCES          = $(example) testBayesTree.cpp
diff --git a/cpp/testGaussianBayesTree.cpp b/cpp/testGaussianBayesTree.cpp
index 800af85cc..861cf1b05 100644
--- a/cpp/testGaussianBayesTree.cpp
+++ b/cpp/testGaussianBayesTree.cpp
@@ -262,6 +262,11 @@ TEST( BayesTree, iSAM_smoother )
 	for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
 	GaussianBayesTree expected(smoother.eliminate(ordering));
 
+	// obtain solution
+	VectorConfig expected_optimized; // no clue...
+	VectorConfig optimized = optimize(actual);
+	CHECK(assert_equal(expected_optimized, optimized));
+
 	CHECK(assert_equal(expected, actual));
 }