separate cpp

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

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

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

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));
 }