isam functionality separated into ISAM class, still within BayesTree

cpp/BayesTree-inl.h

@@ -237,6 +237,10 @@ namespace gtsam {
 	void BayesTree<Conditional>::print(const string& s) const {
 		cout << s << ": size == " << size() << endl;
 		if (nodes_.empty()) return;
+		if (root_.use_count() == 0) {
+			printf("WARNING: Forest...\n");
+			return;
+		}
 		root_->printTree("");
 	}
 
@@ -367,7 +371,7 @@ namespace gtsam {
 	template<class Conditional>
 	template<class Factor>
   pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
-	BayesTree<Conditional>::removePath(sharedClique clique) {
+	ISAM<Conditional>::removePath(sharedClique clique) {
 
 		FactorGraph<Factor> factors;
 		Cliques orphans;
@@ -376,10 +380,10 @@ namespace gtsam {
 		if (clique!=NULL) {
 
 			// remove me
-			removeClique(clique);
+			this->removeClique(clique);
 
 			// remove path above me
-			boost::tie(factors,orphans) = removePath<Factor>(clique->parent_);
+			boost::tie(factors,orphans) = this->removePath<Factor>(clique->parent_);
 
 			// add children to list of orphans (splice also removed them from clique->children_)
 			orphans.splice (orphans.begin(), clique->children_);
@@ -387,8 +391,9 @@ namespace gtsam {
 			// Convert clique to a factor graph, using constructor in FactorGraph
 			FactorGraph<Factor> clique_factors(*clique);
 
-			// add to the list of "imvalidated" factors
+			// add to the list of "invalidated" factors
 			factors.push_back(clique_factors);
+
 		}
 
 		return make_pair(factors,orphans);
@@ -398,7 +403,7 @@ namespace gtsam {
 	// TODO: add to factors and orphans
 	template<class Conditional>
 	template<class Factor>
-  void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
+  void ISAM<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
 		FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
 
 		// process each key of the new factor
@@ -410,11 +415,12 @@ namespace gtsam {
 
 				// remove path from clique to root
 				FactorGraph<Factor> factors1;	Cliques orphans1;
-				boost::tie(factors1,orphans1) = removePath<Factor>(clique);
+				boost::tie(factors1,orphans1) = this->removePath<Factor>(clique);
 
 				// add to global factors and orphans
 				factors.push_back(factors1);
 				orphans.splice (orphans.begin(), orphans1);
+
 			} catch (std::invalid_argument e) {
 			}
 	}
@@ -423,11 +429,11 @@ namespace gtsam {
 	template<class Conditional>
 	template<class Factor>
   pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
-	BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
+	ISAM<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
-
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<Factor> factors;
 		Cliques orphans;
+
 		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
 			this->removeTop<Factor>(factor, factors, orphans);
 
@@ -437,17 +443,24 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
-	void BayesTree<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
+	void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
 
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<Factor> factors;
-		boost::tie(factors, orphans) = removeTop<Factor>(newFactors);
+		boost::tie(factors, orphans) = this->removeTop<Factor>(newFactors);
 
 		// add the factors themselves
 		factors.push_back(newFactors);
 
 		// create an ordering for the new and contaminated factors
-		Ordering ordering = factors.getOrdering();
+		Ordering ordering;
+		if (true) {
+			ordering = factors.getOrdering();
+		} else {
+			list<string> keys = factors.keys();
+			keys.sort(); // todo: correct sorting order?
+			ordering = keys;
+		}
 
 		// eliminate into a Bayes net
 		BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,ordering);
@@ -455,7 +468,7 @@ namespace gtsam {
 		// insert conditionals back in, straight into the topless bayesTree
 		typename BayesNet<Conditional>::const_reverse_iterator rit;
 		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
-			insert(*rit);
+			this->insert(*rit);
 
 		int count = 0;
 		// add orphans to the bottom of the new tree
@@ -472,9 +485,9 @@ namespace gtsam {
 
 	template<class Conditional>
 	template<class Factor>
-	void BayesTree<Conditional>::update(const FactorGraph<Factor>& newFactors) {
+	void ISAM<Conditional>::update(const FactorGraph<Factor>& newFactors) {
 		Cliques orphans;
-		update_internal(newFactors, orphans);
+		this->update_internal<Factor>(newFactors, orphans);
 	}
 
 /* ************************************************************************* */

cpp/BayesTree.h

@@ -104,6 +104,8 @@ namespace gtsam {
 		sharedClique addClique(const sharedConditional& conditional,
 				sharedClique parent_clique = sharedClique());
 
+	protected:
+
 		/** remove a clique: warning, can result in a forest */
 		void removeClique(sharedClique clique);
 
@@ -163,6 +165,27 @@ namespace gtsam {
 		template<class Factor>
 		BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
 
+	}; // BayesTree
+
+	template<class Conditional>
+	class ISAM: public BayesTree<Conditional> {
+
+	public:
+
+		/** Create an empty Bayes Tree */
+		ISAM() : BayesTree<Conditional>() {}
+
+		/** Create a Bayes Tree from a Bayes Net */
+		ISAM(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
+
+		/** Destructor */
+		virtual ~ISAM() {
+		}
+
+		typedef typename BayesTree<Conditional>::sharedClique sharedClique;
+
+		typedef typename BayesTree<Conditional>::Cliques Cliques;
+
 		/**
 		 * Remove path from clique to root and return that path as factors
 		 * plus a list of orphaned subtree roots. Used in removeTop below.
@@ -195,6 +218,6 @@ namespace gtsam {
 		template<class Factor>
 		void update(const FactorGraph<Factor>& newFactors);
 
-	}; // BayesTree
+	}; // ISAM
 
 } /// namespace gtsam

cpp/testBayesTree.cpp

@@ -20,6 +20,10 @@ using namespace boost::assign;
 using namespace gtsam;
 
 typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
+typedef ISAM<SymbolicConditional> SymbolicISAM;
+
+//template class BayesTree<SymbolicConditional>; // todo: needed?
+//template class ISAM<SymbolicConditional>;
 
 /* ************************************************************************* */
 // SLAM example from RSS sqrtSAM paper
@@ -30,9 +34,9 @@ SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
 		l2(new SymbolicConditional("l2","x1","x3"));
 
 // Bayes Tree for sqrtSAM example
-SymbolicBayesTree createSlamSymbolicBayesTree(){
+SymbolicISAM createSlamSymbolicBayesTree(){
 	// Create using insert
-	SymbolicBayesTree bayesTree_slam;
+	SymbolicISAM bayesTree_slam;
 	bayesTree_slam.insert(x3);
 	bayesTree_slam.insert(x2);
 	bayesTree_slam.insert(x1);
@@ -54,8 +58,8 @@ SymbolicConditional::shared_ptr
 	X(new SymbolicConditional("X", "E"));
 
 // Bayes Tree for Asia example
-SymbolicBayesTree createAsiaSymbolicBayesTree() {
+SymbolicISAM createAsiaSymbolicBayesTree() {
-	SymbolicBayesTree bayesTree;
+	SymbolicISAM bayesTree;
 	bayesTree.insert(B);
 	bayesTree.insert(L);
 	bayesTree.insert(E);
@@ -124,7 +128,7 @@ TEST( BayesTree, removePath )
 			D(new SymbolicConditional("D", "C")),
 			E(new SymbolicConditional("E", "B")),
 			F(new SymbolicConditional("F", "E"));
-	SymbolicBayesTree bayesTree;
+	SymbolicISAM bayesTree;
 	bayesTree.insert(A);
 	bayesTree.insert(B);
 	bayesTree.insert(C);
@@ -138,11 +142,11 @@ TEST( BayesTree, removePath )
 	expected.push_factor("A","B");
 	expected.push_factor("A");
 	expected.push_factor("A","C");
-	SymbolicBayesTree::Cliques expectedOrphans;
+	SymbolicISAM::Cliques expectedOrphans;
   expectedOrphans += bayesTree["D"], bayesTree["E"];
 
 	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
+	SymbolicISAM::Cliques orphans;
 	boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
   CHECK(assert_equal(expectedOrphans, orphans));
@@ -150,7 +154,7 @@ TEST( BayesTree, removePath )
   // remove E: factor graph with EB; E|B removed from second orphan tree
 	SymbolicFactorGraph expected2;
   expected2.push_factor("B","E");
-  SymbolicBayesTree::Cliques expectedOrphans2;
+  SymbolicISAM::Cliques expectedOrphans2;
   expectedOrphans2 += bayesTree["F"];
 
   boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
@@ -161,11 +165,11 @@ TEST( BayesTree, removePath )
 /* ************************************************************************* */
 TEST( BayesTree, removePath2 )
 {
-	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
 
 	// Call remove-path with clique B
 	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
+	SymbolicISAM::Cliques orphans;
   boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["B"]);
 
 	// Check expected outcome
@@ -174,7 +178,7 @@ TEST( BayesTree, removePath2 )
 	expected.push_factor("B","L");
 	expected.push_factor("B");
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
-	SymbolicBayesTree::Cliques expectedOrphans;
+	SymbolicISAM::Cliques expectedOrphans;
   expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"];
   CHECK(assert_equal(expectedOrphans, orphans));
 }
@@ -182,11 +186,11 @@ TEST( BayesTree, removePath2 )
 /* ************************************************************************* */
 TEST( BayesTree, removePath3 )
 {
-	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
 
 	// Call remove-path with clique S
 	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
+	SymbolicISAM::Cliques orphans;
   boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["S"]);
 
 	// Check expected outcome
@@ -196,22 +200,22 @@ TEST( BayesTree, removePath3 )
 	expected.push_factor("B");
 	expected.push_factor("L","B","S");
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
-	SymbolicBayesTree::Cliques expectedOrphans;
+	SymbolicISAM::Cliques expectedOrphans;
   expectedOrphans += bayesTree["T"], bayesTree["X"];
   CHECK(assert_equal(expectedOrphans, orphans));
 }
 
 /* ************************************************************************* */
-TEST( BayesTree, removeTop )
+TEST( ISAM, removeTop )
 {
-	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
 
 	// create a new factor to be inserted
 	boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
 
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
+	SymbolicISAM::Cliques orphans;
 	bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
 
 	// Check expected outcome
@@ -221,26 +225,26 @@ TEST( BayesTree, removeTop )
 	expected.push_factor("B");
 	expected.push_factor("L","B","S");
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
-	SymbolicBayesTree::Cliques expectedOrphans;
+	SymbolicISAM::Cliques expectedOrphans;
   expectedOrphans += bayesTree["T"], bayesTree["X"];
   CHECK(assert_equal(expectedOrphans, orphans));
 
   // Try removeTop again with a factor that should not change a thing
 	boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
 	FactorGraph<SymbolicFactor> factors2;
-	SymbolicBayesTree::Cliques orphans2;
+	SymbolicISAM::Cliques orphans2;
 	bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
 	SymbolicFactorGraph expected2;
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
-	SymbolicBayesTree::Cliques expectedOrphans2;
+	SymbolicISAM::Cliques expectedOrphans2;
   CHECK(assert_equal(expectedOrphans2, orphans2));
 }
 
 
 /* ************************************************************************* */
-TEST( BayesTree, removeTop2 )
+TEST( ISAM, removeTop2 )
 {
-	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
 
 	// create two factors to be inserted
 	SymbolicFactorGraph newFactors;
@@ -249,7 +253,7 @@ TEST( BayesTree, removeTop2 )
 
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
+	SymbolicISAM::Cliques orphans;
 	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
 
 	// Check expected outcome
@@ -259,15 +263,15 @@ TEST( BayesTree, removeTop2 )
 	expected.push_factor("B");
 	expected.push_factor("L","B","S");
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
-	SymbolicBayesTree::Cliques expectedOrphans;
+	SymbolicISAM::Cliques expectedOrphans;
   expectedOrphans += bayesTree["T"], bayesTree["X"];
 	CHECK(assert_equal(expectedOrphans, orphans));
 }
 
 /* ************************************************************************* */
-TEST( BayesTree, iSAM )
+TEST( ISAM, iSAM )
 {
-	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+	SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
 
 	// Now we modify the Bayes tree by inserting a new factor over B and S
 
@@ -279,7 +283,7 @@ TEST( BayesTree, iSAM )
 		B_(new SymbolicConditional("B", "E", "L", "S"));
 
 	// Create expected Bayes tree
-	SymbolicBayesTree expected;
+	SymbolicISAM expected;
 	expected.insert(S_);
 	expected.insert(L_);
 	expected.insert(E_);
@@ -298,19 +302,19 @@ TEST( BayesTree, iSAM )
 	// Check whether the same
   CHECK(assert_equal(expected,bayesTree));
 }
+
 /* ************************************************************************* */
-TEST( BayesTree, iSAM_slam )
+TEST( ISAM, iSAM_slam )
 {
 	// Create using insert
-	SymbolicBayesTree bayesTree_slam = createSlamSymbolicBayesTree();
+	SymbolicISAM bayesTree_slam = createSlamSymbolicBayesTree();
 
 	//New conditionals for the expected Bayes tree
-
 	SymbolicConditional::shared_ptr
 			l1_(new SymbolicConditional("l1","x1","x2","x3"));
 
 	// Create expected Bayes tree
-	SymbolicBayesTree expected_slam;
+	SymbolicISAM expected_slam;
 	expected_slam.insert(x3);
 	expected_slam.insert(x2);
 	expected_slam.insert(x1);