removeTop/Path now returns a BayesNet to avoid converting the conditionals to a factor graph

cpp/BayesTree-inl.h

@@ -397,18 +397,12 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Conditional>
-	template<class Factor>
 	void BayesTree<Conditional>::removePath(sharedClique clique,
-			FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
+			BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) {
 
 		// base case is NULL, if so we do nothing and return empties above
 		if (clique!=NULL) {
 
-#if 0
-			printf("++++++ removing\n");
-			clique->print();
-#endif
-
 			// remove the clique from orphans in case it has been added earlier
 			orphans.remove(clique);
 
@@ -416,32 +410,20 @@ namespace gtsam {
 			this->removeClique(clique);
 
 			// remove path above me
-			this->removePath<Factor>(clique->parent_, factors, orphans);
+			this->removePath(clique->parent_, bn, orphans);
 
 			// add children to list of orphans (splice also removed them from clique->children_)
 			orphans.splice (orphans.begin(), clique->children_);
 
-			// Convert clique to a factor graph, using constructor in FactorGraph
+			bn.push_back(*clique);
-			FactorGraph<Factor> clique_factors(*clique);
-
-			// add to the list of "invalidated" factors
-			factors.push_back(clique_factors);
-
-#if 0
-			printf("++++++ factors\n");
-			factors.print();
-			printf("++++++ orphans\n");
-			orphans.print();
-#endif
 
 		}
 	}
 
 	/* ************************************************************************* */
 	template<class Conditional>
-	template<class Factor>
   void BayesTree<Conditional>::removeTop(const list<Symbol>& keys,
-  		FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
+  		BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) {
 
 		// process each key of the new factor
 		BOOST_FOREACH(const Symbol& key, keys)
@@ -450,7 +432,7 @@ namespace gtsam {
 				sharedClique clique = (*this)[key];
 
 				// remove path from clique to root
-				this->removePath<Factor>(clique, factors, orphans);
+				this->removePath(clique, bn, orphans);
 
 			} catch (std::invalid_argument e) {
 			}

cpp/BayesTree.h

@@ -181,16 +181,13 @@ namespace gtsam {
 		 * Remove path from clique to root and return that path as factors
 		 * plus a list of orphaned subtree roots. Used in removeTop below.
 		 */
-		template<class Factor>
+		void removePath(sharedClique clique, BayesNet<Conditional>& bn, Cliques& orphans);
-		void removePath(sharedClique clique, FactorGraph<Factor> &factors, Cliques& orphans);
 
 		/**
 		 * Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
 		 * Factors and orphans are added to the in/out arguments.
 		 */
-		template<class Factor>
+		void removeTop(const std::list<Symbol>& keys,	BayesNet<Conditional>& bn, Cliques& orphans);
-		void removeTop(const std::list<Symbol>& keys,
-				FactorGraph<Factor> &factors, Cliques& orphans);
 
 	}; // BayesTree
 

cpp/ISAM-inl.h

@@ -31,8 +31,9 @@ namespace gtsam {
 	void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
 
 		// Remove the contaminated part of the Bayes tree
-		FactorGraph<Factor> factors;
+		BayesNet<Conditional> bn;
-		removeTop(newFactors.keys(), factors, orphans);
+		removeTop(newFactors.keys(), bn, orphans);
+		FactorGraph<Factor> factors(bn);
 
 		// add the factors themselves
 		factors.push_back(newFactors);

cpp/ISAM2-inl.h

@@ -137,9 +137,6 @@ namespace gtsam {
 	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
 			const Config& config, Cliques& orphans) {
 
-#if 1 // 0=skip most, do batch
-
-		FactorGraph<GaussianFactor> affectedFactors;
 		list<Symbol> newFactorsKeys = newFactors.keys();
 
 #if 1 // 0=relinearize all in each step
@@ -163,7 +160,8 @@ namespace gtsam {
 #endif
 
 		// remove affected factors
-		this->removeTop(keysToBeRemoved, affectedFactors, orphans);
+		BayesNet<GaussianConditional> affectedBayesNet;
+		this->removeTop(keysToBeRemoved, affectedBayesNet, orphans);
 
 		// selectively update the linearization point
 		VectorConfig selected_delta;
@@ -174,21 +172,12 @@ namespace gtsam {
 		linPoint_ = expmap(linPoint_, selected_delta);
 
 		// relinearize the affected factors ...
-		list<Symbol> affectedKeys = affectedFactors.keys();
+		list<Symbol> affectedKeys = affectedBayesNet.ordering(); // all keys in conditionals, there cannot be others because path to root included
 		FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
 
 		// ... add the cached intermediate results from the boundary of the orphans ...
 		FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
 		factors.push_back(cachedBoundary);
-#else
-		// todo - debug only: batch operation
-		FactorGraph<GaussianFactor> affectedFactors;
-		list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
-		this->removeTop(keysToBeRemoved, affectedFactors, orphans);
-		this->print("---------------");
-		linPoint_ = expmap(linPoint_, delta_); // todo-debug only
-		FactorGraph<GaussianFactor> factors = nonlinearFactors_.linearize(linPoint_);
-#endif
 
 		// add new variables
 		linPoint_.insert(config);

cpp/testBayesTree.cpp

@@ -155,9 +155,10 @@ TEST( BayesTree, removePath )
 	SymbolicBayesTree::Cliques expectedOrphans;
   expectedOrphans += bayesTree["D"], bayesTree["E"];
 
-	FactorGraph<SymbolicFactor> factors;
+  BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-	bayesTree.removePath<SymbolicFactor>(bayesTree["C"], factors, orphans);
+	bayesTree.removePath(bayesTree["C"], bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
   CHECK(assert_equal(expectedOrphans, orphans));
 
@@ -167,9 +168,10 @@ TEST( BayesTree, removePath )
   SymbolicBayesTree::Cliques expectedOrphans2;
   expectedOrphans2 += bayesTree["F"];
 
-	FactorGraph<SymbolicFactor> factors2;
+  BayesNet<SymbolicConditional> bn2;
 	SymbolicBayesTree::Cliques orphans2;
-  bayesTree.removePath<SymbolicFactor>(bayesTree["E"], factors2, orphans2);
+  bayesTree.removePath(bayesTree["E"], bn2, orphans2);
+  FactorGraph<SymbolicFactor> factors2(bn2);
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
   CHECK(assert_equal(expectedOrphans2, orphans2));
 }
@@ -180,9 +182,10 @@ TEST( BayesTree, removePath2 )
 	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
 
 	// Call remove-path with clique B
-	FactorGraph<SymbolicFactor> factors;
+	BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-  bayesTree.removePath<SymbolicFactor>(bayesTree["B"], factors, orphans);
+  bayesTree.removePath(bayesTree["B"], bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -201,9 +204,10 @@ TEST( BayesTree, removePath3 )
 	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
 
 	// Call remove-path with clique S
-	FactorGraph<SymbolicFactor> factors;
+	BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-  bayesTree.removePath<SymbolicFactor>(bayesTree["S"], factors, orphans);
+  bayesTree.removePath(bayesTree["S"], bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -226,9 +230,10 @@ TEST( BayesTree, removeTop )
 	boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
 
 	// Remove the contaminated part of the Bayes tree
-	FactorGraph<SymbolicFactor> factors;
+	BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-	bayesTree.removeTop<SymbolicFactor>(newFactor->keys(), factors, orphans);
+	bayesTree.removeTop(newFactor->keys(), bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -243,9 +248,10 @@ TEST( BayesTree, removeTop )
 
   // Try removeTop again with a factor that should not change a thing
 	boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
-	FactorGraph<SymbolicFactor> factors2;
+	BayesNet<SymbolicConditional> bn2;
 	SymbolicBayesTree::Cliques orphans2;
-	bayesTree.removeTop<SymbolicFactor>(newFactor2->keys(), factors2, orphans2);
+	bayesTree.removeTop(newFactor2->keys(), bn2, orphans2);
+	FactorGraph<SymbolicFactor> factors2(bn2);
 	SymbolicFactorGraph expected2;
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
 	SymbolicBayesTree::Cliques expectedOrphans2;
@@ -263,9 +269,10 @@ TEST( BayesTree, removeTop2 )
 	newFactors.push_factor("S");
 
 	// Remove the contaminated part of the Bayes tree
-	FactorGraph<SymbolicFactor> factors;
+	BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-	bayesTree.removeTop<SymbolicFactor>(newFactors.keys(), factors, orphans);
+	bayesTree.removeTop(newFactors.keys(), bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -298,9 +305,10 @@ TEST( BayesTree, removeTop3 )
 	// remove all
 	list<Symbol> keys;
 	keys += "l5", "x2", "x3", "x4";
-	FactorGraph<SymbolicFactor> factors;
+	BayesNet<SymbolicConditional> bn;
 	SymbolicBayesTree::Cliques orphans;
-	bayesTree.removeTop<SymbolicFactor>(keys, factors, orphans);
+	bayesTree.removeTop(keys, bn, orphans);
+	FactorGraph<SymbolicFactor> factors(bn);
 
 	CHECK(orphans.size() == 0);
 }