From 19dc8bf4b17c489b15c958256f10fdf60894f68c Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 20 Jan 2010 04:23:35 +0000
Subject: [PATCH] Fixed size() crash when BayesTree empty, added clear()
 function to BayesTree, added and removed #includes to fix link errors

---
 cpp/BayesNet-inl.h    |  4 ++--
 cpp/BayesTree-inl.h   |  9 +++++++++
 cpp/BayesTree.h       | 10 +++++++++-
 cpp/ISAM-inl.h        |  7 +++----
 cpp/ISAM.h            |  2 ++
 cpp/LieConfig-inl.h   |  2 +-
 cpp/testBayesTree.cpp | 12 ++++++++++++
 7 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/cpp/BayesNet-inl.h b/cpp/BayesNet-inl.h
index 9d07baf1a..ad7d1f618 100644
--- a/cpp/BayesNet-inl.h
+++ b/cpp/BayesNet-inl.h
@@ -13,8 +13,8 @@ using namespace boost::assign;
 
 #include "Ordering.h"
 #include "BayesNet.h"
-#include "FactorGraph-inl.h"
-#include "Conditional.h"
+//#include "FactorGraph-inl.h"
+//#include "Conditional.h"
 
 using namespace std;
 
diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index d2b5db148..6807b00e0 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -12,6 +12,7 @@ using namespace boost::assign;
 
 #include "Conditional.h"
 #include "BayesTree.h"
+#include "Ordering.h"
 #include "inference-inl.h"
 
 namespace gtsam {
@@ -386,6 +387,14 @@ namespace gtsam {
 		return eliminate<Factor,Conditional>(fg,ordering);
 	}
 
+	/* ************************************************************************* */
+	template<class Conditional>
+	void BayesTree<Conditional>::clear() {
+		// Remove all nodes and clear the root pointer
+		nodes_.clear();
+		root_.reset();
+	}
+
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index aeb0533a0..2ad5f5d70 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -136,7 +136,10 @@ namespace gtsam {
 
 		/** number of cliques */
 		inline size_t size() const {
-			return root_->treeSize();
+			if(root_)
+				return root_->treeSize();
+			else
+				return 0;
 		}
 
 		/** return root clique */
@@ -169,6 +172,11 @@ namespace gtsam {
 		template<class Factor>
 		BayesNet<Conditional> jointBayesNet(const Symbol& key1, const Symbol& key2) const;
 
+		/**
+		 * Remove all nodes
+		 */
+		void clear();
+
 		/**
 		 * Remove path from clique to root and return that path as factors
 		 * plus a list of orphaned subtree roots. Used in removeTop below.
diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h
index 53bce6cfe..a05aae622 100644
--- a/cpp/ISAM-inl.h
+++ b/cpp/ISAM-inl.h
@@ -9,8 +9,8 @@
 using namespace boost::assign;
 
 #include "Conditional.h"
-#include "BayesTree-inl.h"
 #include "ISAM.h"
+#include "BayesTree-inl.h"
 
 namespace gtsam {
 
@@ -22,7 +22,8 @@ namespace gtsam {
 
 	/** Create a Bayes Tree from a Bayes Net */
 	template<class Conditional>
-	ISAM<Conditional>::ISAM(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
+	ISAM<Conditional>::ISAM(const BayesNet<Conditional>& bayesNet) :
+	  BayesTree<Conditional>(bayesNet) {}
 
 	/* ************************************************************************* */
 	template<class Conditional>
@@ -74,7 +75,5 @@ namespace gtsam {
 		this->update_internal<Factor>(newFactors, orphans);
 	}
 
-/* ************************************************************************* */
-
 }
 /// namespace gtsam
diff --git a/cpp/ISAM.h b/cpp/ISAM.h
index de19dfa04..a43c6dc66 100644
--- a/cpp/ISAM.h
+++ b/cpp/ISAM.h
@@ -19,6 +19,8 @@
 #include "FactorGraph.h"
 #include "BayesNet.h"
 #include "BayesTree.h"
+#include "VectorConfig.h"
+#include "NonlinearFactorGraph.h"
 
 namespace gtsam {
 
diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h
index 267d20e67..d6193e4c1 100644
--- a/cpp/LieConfig-inl.h
+++ b/cpp/LieConfig-inl.h
@@ -85,7 +85,7 @@ namespace gtsam {
 				const Vector& dj = delta[jkey];
 				newConfig.insert(j, expmap(pj,dj));
 			} else
-			newConfig.insert(j, pj);
+				newConfig.insert(j, pj);
 		}
 		return newConfig;
 	}
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 061fb2d6f..950a11536 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -111,6 +111,18 @@ TEST( BayesTree, constructor )
 	CHECK(assert_equal(bayesTree,bayesTree2));
 }
 
+/* ************************************************************************* */
+TEST(BayesTree, clear)
+{
+	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	bayesTree.clear();
+
+	SymbolicBayesTree expected;
+
+	// Check whether cleared BayesTree is equal to a new BayesTree
+	CHECK(assert_equal(expected, bayesTree));
+}
+
 /* ************************************************************************* *
 Bayes Tree for testing conversion to a forest of orphans needed for incremental.
        A,B