fixed junction tree unit tests

cpp/BayesTree-inl.h

@@ -48,8 +48,10 @@ namespace gtsam {
 	template<class Conditional>
 	void BayesTree<Conditional>::Clique::print(const string& s) const {
 			cout << s;
-			BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
+			BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) {
+				conditional->print("conditioanl");
 				cout << " " << (string)(conditional->key());
+			}
 			if (!separator_.empty()) {
 				cout << " :";
 				BOOST_FOREACH(const Symbol& key, separator_)
@@ -349,6 +351,34 @@ namespace gtsam {
 			insert(*rit, index);
 	}
 
+	/* ************************************************************************* */
+	template<class Conditional>
+	BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees) {
+		if (bayesNet.size() == 0)
+			throw invalid_argument("BayesTree::insert: empty bayes net!");
+
+		// get the roots of child subtrees and merge their nodes_
+		list<sharedClique> childRoots;
+		BOOST_FOREACH(const BayesTree<Conditional>& subtree, subtrees) {
+			nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end());
+			childRoots.push_back(subtree.root());
+		}
+
+		// create a new clique and add all the conditionals to the clique
+		sharedClique new_clique;
+		typename BayesNet<Conditional>::sharedConditional conditional;
+		BOOST_REVERSE_FOREACH(conditional, bayesNet) {
+			if (!new_clique.get()) {
+				new_clique = addClique(conditional,childRoots);
+			} else {
+				nodes_.insert(make_pair(conditional->key(), new_clique));
+				new_clique->push_front(conditional);
+			}
+		}
+
+		root_ = new_clique;
+	}
+
 	/* ************************************************************************* */
 	template<class Conditional>
 	void BayesTree<Conditional>::print(const string& s) const {
@@ -358,7 +388,6 @@ namespace gtsam {
 		}
 		cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() <<  endl;
 		if (nodes_.empty()) return;
-		printf("printing tree!\n");
 		root_->printTree("");
 	}
 
@@ -430,6 +459,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	//TODO: remove this function after removing TSAM.cpp
 	template<class Conditional>
 	typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::insert(
 			const BayesNet<Conditional>& bayesNet, list<sharedClique>& children, bool isRootClique)

cpp/BayesTree.h

@@ -151,6 +151,12 @@ namespace gtsam {
 		/** Create a Bayes Tree from a Bayes Net */
 		BayesTree(const BayesNet<Conditional>& bayesNet);
 
+		/**
+		 * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
+		 * new root clique and the subtrees are connected to the root clique.
+		 */
+		BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees);
+
 		/** Destructor */
 		virtual ~BayesTree() {
 		}

cpp/GaussianConditional.cpp

@@ -64,26 +64,46 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const {
 	if (parents_.size() != p->parents_.size()) return false;
 
 	// check if R_ and d_ are equal up to a sign
+//	for (size_t i=0; i<d_.size(); i++) {
+//		Vector row1 = row_(R_, i);
+//		Vector row2 = row_(p->R_, i);
+//		if (!((::equal_with_abs_tol(row1, row2, tol)      && fabs(d_(i) - p->d_(i)) < tol) ||
+//	  			(::equal_with_abs_tol(row1, row2*(-1), tol) && fabs(d_(i) + p->d_(i)) < tol)))
+//			return false;
+//
+//		float sign = ::equal_with_abs_tol(row1, row2, tol) ? 1 : -1;
+//
+//		// check if the matrices are the same
+//		// iterate over the parents_ map
+//		for (it = parents_.begin(); it != parents_.end(); it++) {
+//			Parents::const_iterator it2 = p->parents_.find(it->first);
+//			if (it2 != p->parents_.end()) {
+//				if (!::equal_with_abs_tol(row_(it->second*sign, i), row_(it2->second,i), tol))
+//					return false;
+//			} else
+//				return false;
+//		}
+//
+//	}
+	// check if R_ and d_ are linear independent
 	for (size_t i=0; i<d_.size(); i++) {
-		Vector row1 = row_(R_, i);
+		list<Vector> rows1; rows1.push_back(row_(R_, i));
-		Vector row2 = row_(p->R_, i);
+		list<Vector> rows2; rows2.push_back(row_(p->R_, i));
-		if (!((::equal_with_abs_tol(row1, row2, tol)      && fabs(d_(i) - p->d_(i)) < tol) ||
-	  			(::equal_with_abs_tol(row1, row2*(-1), tol) && fabs(d_(i) + p->d_(i)) < tol)))
-			return false;
-
-		float sign = ::equal_with_abs_tol(row1, row2, tol) ? 1 : -1;
 
 		// check if the matrices are the same
 		// iterate over the parents_ map
 		for (it = parents_.begin(); it != parents_.end(); it++) {
 			Parents::const_iterator it2 = p->parents_.find(it->first);
 			if (it2 != p->parents_.end()) {
-				if (!::equal_with_abs_tol(row_(it->second*sign, i), row_(it2->second,i), tol))
+				rows1.push_back(row_(it->second, i));
-					return false;
+				rows2.push_back(row_(it2->second,i));
 			} else
 				return false;
 		}
 
+		Vector row1 = concatVectors(rows1);
+		Vector row2 = concatVectors(rows2);
+		if (!linear_dependent(row1, row2, tol)) return false;
 	}
 
 	// check if sigmas are equal

cpp/GaussianFactor.cpp

@@ -379,9 +379,9 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
 #include <boost/numeric/ublas/io.hpp>
 #include <boost/numeric/ublas/matrix_proxy.hpp>
 
-pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
+pair<GaussianBayesNet, GaussianFactor::shared_ptr>
 GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
-		        const Ordering& frontal, const Ordering& separator,
+		        const Ordering& frontals, const Ordering& separators,
 		        const Dimensions& dimensions) {
 	bool verbose = false;
 
@@ -389,40 +389,91 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
 	if (verbose) model->print("Before QR");
 	SharedDiagonal noiseModel = model->QR(Ab);
 	if (verbose) model->print("After QR");
+//	gtsam::print(Ab, "Ab after QR");
 
 	// get dimensions of the eliminated variable
 	// TODO: this is another map find that should be avoided !
-	size_t n1 = dimensions.at(frontal.front()), n = Ab.size2() - 1;
+	size_t n1 = dimensions.at(frontals.front()), n = Ab.size2() - 1;
+
+	// Get alias to augmented RHS d
+	ublas::matrix_column<Matrix> d(Ab,n);
+
+//	// create base conditional Gaussian
+//	GaussianConditional::shared_ptr conditional(new GaussianConditional(frontals.front(),
+//			sub(d,  0, n1),                   // form d vector
+//			sub(Ab, 0, n1, 0, n1),            // form R matrix
+//			sub(noiseModel->sigmas(),0,n1))); // get standard deviations
+//
+//	// extract the block matrices for parents in both CG and LF
+//	GaussianFactor::shared_ptr factor(new GaussianFactor);
+//	size_t j = n1;
+//	BOOST_FOREACH(const Symbol& cur_key, separators) {
+//		size_t dim = dimensions.at(cur_key); // TODO avoid find !
+//		conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
+//		factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
+//		j+=dim;
+//	}
+//
+//	// Set sigmas
+//	// set the right model here
+//	if (noiseModel->isConstrained())
+//		factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank));
+//	else
+//		factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank));
+//
+//	// extract ds vector for the new b
+//	factor->set_b(sub(d, n1, maxRank));
+//
+//	return make_pair(conditional, factor);
+
+	// extract the conditionals
+	GaussianBayesNet bn;
+	size_t n0 = 0;
+	Ordering::const_iterator itFrontal1 = frontals.begin(), itFrontal2;
+	for(; itFrontal1!=frontals.end(); itFrontal1++) {
+		n1 = n0 + dimensions.at(*itFrontal1);
+		// create base conditional Gaussian
+		GaussianConditional::shared_ptr conditional(new GaussianConditional(*itFrontal1,
+				sub(d,  n0, n1),                   // form d vector
+				sub(Ab, n0, n1, n0, n1),           // form R matrix
+				sub(noiseModel->sigmas(),n0,n1))); // get standard deviations
+
+		// add parents to the conditional
+		itFrontal2 = itFrontal1;
+		itFrontal2 ++;
+		size_t j = n1;
+		for (; itFrontal2!=frontals.end(); itFrontal2++) {
+			size_t dim = dimensions.at(*itFrontal2);
+			conditional->add(*itFrontal2, sub(Ab, n0, n1, j, j+dim));
+			j+=dim;
+		}
+		BOOST_FOREACH(const Symbol& cur_key, separators) {
+			size_t dim = dimensions.at(cur_key);
+			conditional->add(cur_key, sub(Ab, n0, n1, j, j+dim));
+			j+=dim;
+		}
+		n0 = n1;
+		bn.push_back(conditional);
+	}
 
 	// if m<n1, this factor cannot be eliminated
 	size_t maxRank = noiseModel->dim();
 	if (maxRank<n1) {
 		cout << "Perhaps your factor graph is singular." << endl;
 		cout << "Here are the keys involved in the factor now being eliminated:" << endl;
-		separator.print("Keys");
+		separators.print("Keys");
-		cout << "The first key, '" << (string)frontal.front() << "', corresponds to the variable being eliminated" << endl;
+		cout << "The first key, '" << (string)frontals.front() << "', corresponds to the variable being eliminated" << endl;
 		throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
 	}
 
-	// Get alias to augmented RHS d
+	// extract the new factor
-	ublas::matrix_column<Matrix> d(Ab,n);
-
-	// create base conditional Gaussian
-	GaussianConditional::shared_ptr conditional(new GaussianConditional(frontal.front(),
-			sub(d,  0, n1),                   // form d vector
-			sub(Ab, 0, n1, 0, n1),            // form R matrix
-			sub(noiseModel->sigmas(),0,n1))); // get standard deviations
-
-	// extract the block matrices for parents in both CG and LF
 	GaussianFactor::shared_ptr factor(new GaussianFactor);
 	size_t j = n1;
-	BOOST_FOREACH(const Symbol& cur_key, separator)
+	BOOST_FOREACH(const Symbol& cur_key, separators) {
-		if (cur_key!=frontal.front()) {
+		size_t dim = dimensions.at(cur_key); // TODO avoid find !
-			size_t dim = dimensions.at(cur_key); // TODO avoid find !
+		factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
-			conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
+		j+=dim;
-			factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
+	}
-			j+=dim;
-		}
 
 	// Set sigmas
 	// set the right model here
@@ -434,9 +485,20 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
 	// extract ds vector for the new b
 	factor->set_b(sub(d, n1, maxRank));
 
-	return make_pair(conditional, factor);
+	return make_pair(bn, factor);
+
 }
 
+/* ************************************************************************* */
+pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
+GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
+		        const Symbol& frontal, const Ordering& separator,
+		        const Dimensions& dimensions) {
+	Ordering frontals; frontals += frontal;
+	pair<GaussianBayesNet, shared_ptr> ret =
+			eliminateMatrix(Ab, model, frontals, separator, dimensions);
+	return make_pair(*ret.first.begin(), ret.second);
+}
 /* ************************************************************************* */
 pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
 GaussianFactor::eliminate(const Symbol& key) const
@@ -451,8 +513,7 @@ GaussianFactor::eliminate(const Symbol& key) const
 	}
 
 	// create an internal ordering that eliminates key first
-	Ordering frontal, ordering;
+	Ordering ordering;
-	frontal += key;
 	ordering += key;
 	BOOST_FOREACH(const Symbol& k, keys())
 		if (k != key) ordering += k;
@@ -462,7 +523,7 @@ GaussianFactor::eliminate(const Symbol& key) const
 
 	// TODO: this is where to split
 	ordering.pop_front();
-	return eliminateMatrix(Ab, model_, frontal, ordering, dimensions());
+	return eliminateMatrix(Ab, model_, key, ordering, dimensions());
 }
 
 /* ************************************************************************* */

cpp/GaussianFactor.h

@@ -21,6 +21,7 @@
 #include "VectorConfig.h"
 #include "SharedDiagonal.h"
 #include "GaussianConditional.h" // Needed for MATLAB
+#include "GaussianBayesNet.h"
 #include "SymbolMap.h"
 
 namespace gtsam {
@@ -250,11 +251,17 @@ public:
 	 * Performs elimination given an augmented matrix
 	 * @param
 	 */
-	static std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
+	static std::pair<GaussianBayesNet, shared_ptr>
 	eliminateMatrix(Matrix& Ab, SharedDiagonal model,
 			const Ordering& frontal, const Ordering& separator,
 			const Dimensions& dimensions);
 
+	static std::pair<GaussianConditional::shared_ptr, shared_ptr>
+	eliminateMatrix(Matrix& Ab, SharedDiagonal model,
+			const Symbol& frontal, const Ordering& separator,
+			const Dimensions& dimensions);
+
+
 	/**
 	 * Take the factor f, and append to current matrices. Not very general.
 	 * @param f linear factor graph

cpp/GaussianFactorGraph.cpp

@@ -216,7 +216,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
 	}
 
 	// add the keys to the rendering
-	Ordering frontal, render; frontal += key; render += key;
+	Ordering render; render += key;
 	BOOST_FOREACH(const Symbol& k, separator)
 			if (k != key) render += k;
 
@@ -230,7 +230,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
 	GaussianConditional::shared_ptr conditional;
 	render.pop_front();
 	boost::tie(conditional, factor) =
-			GaussianFactor::eliminateMatrix(Ab, model, frontal, render, dimensions);
+			GaussianFactor::eliminateMatrix(Ab, model, key, render, dimensions);
 
 	// add new factor on separator back into the graph
 	if (!factor->empty()) push_back(factor);
@@ -255,16 +255,31 @@ GaussianFactorGraph::eliminate(const Ordering& ordering, bool old)
 GaussianBayesNet
 GaussianFactorGraph::eliminateFrontals(const Ordering& frontals)
 {
-	Matrix Ab; SharedDiagonal model;
+	// find the factors that contain at least one of the frontal variables
 	Dimensions dimensions = this->dimensions();
-	boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, keys(), dimensions);
+
+	// collect separator
+	Ordering separator;
+	set<Symbol> frontal_set(frontals.begin(), frontals.end());
+	BOOST_FOREACH(const Symbol& key, this->keys()) {
+		if (frontal_set.find(key) == frontal_set.end())
+			separator.push_back(key);
+	}
+
+	Matrix Ab; SharedDiagonal model;
+	Ordering ord = frontals;
+	ord.insert(ord.end(), separator.begin(), separator.end());
+	boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, ord, dimensions);
 
 	// eliminate that joint factor
 	GaussianFactor::shared_ptr factor;
-//	GaussianConditional::shared_ptr conditional;
 	GaussianBayesNet bn;
-//	boost::tie(bn, factor) =
+	boost::tie(bn, factor) =
-//			GaussianFactor::eliminateMatrix(Ab, model, frontals, dimensions);
+			GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions);
+
+	// add new factor on separator back into the graph
+	*this = GaussianFactorGraph();
+	if (!factor->empty()) push_back(factor);
 
 	return bn;
 }

cpp/JunctionTree-inl.h

@@ -45,7 +45,6 @@ namespace gtsam {
 	template <class FG>
 	void JunctionTree<FG>::Clique::print(const string& indent) const {
 		// FG::print(indent);
-		cout << "kai1" << endl;
 		cout << indent;
 		BOOST_FOREACH(const Symbol& key, frontal_)
 		cout << (string)key << " ";
@@ -70,7 +69,6 @@ namespace gtsam {
 		SymbolicFactorGraph sfg(fg);
 		SymbolicBayesNet sbn = sfg.eliminate(ordering);
 		BayesTree<SymbolicConditional> sbt(sbn);
-		sbt.print("sbt");
 
 		// distribtue factors
 		root_ = distributeFactors(fg, sbt.root());
@@ -106,39 +104,49 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template <class FG> template <class Conditional>
-	pair<FG, typename BayesTree<Conditional>::sharedClique>
+	pair<FG, BayesTree<Conditional> >
-	JunctionTree<FG>::eliminateOneClique(sharedClique current, BayesTree<Conditional>& bayesTree) {
+	JunctionTree<FG>::eliminateOneClique(sharedClique current) {
+
+//		current->frontal_.print("current clique:");
+
 		typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
 		FG fg; // factor graph will be assembled from local factors and marginalized children
-		list<sharedBtreeClique> children;
+		list<BayesTree<Conditional> > children;
 		fg.push_back(*current); // add the local factor graph
+
+//		BOOST_FOREACH(const typename FG::sharedFactor& factor_, fg)
+//			Ordering(factor_->keys()).print("local factor:");
+
 		BOOST_FOREACH(sharedClique& child, current->children_) {
 			// receive the factors from the child and its clique point
-			FG fgChild; sharedBtreeClique cliqueChild;
+			FG fgChild; BayesTree<Conditional> childTree;
-			boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree);
+			boost::tie(fgChild, childTree) = eliminateOneClique<Conditional>(child);
-			if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!");
+
+//			BOOST_FOREACH(const typename FG::sharedFactor& factor_, fgChild)
+//				Ordering(factor_->keys()).print("factor from child:");
 
 			fg.push_back(fgChild);
-			children.push_back(cliqueChild);
+			children.push_back(childTree);
 		}
 
 		// eliminate the combined factors
 		// warning: fg is being eliminated in-place and will contain marginal afterwards
-//		BayesNet<Conditional> bn = fg.eliminate(current->frontal_);
 		BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
 
 		// create a new clique corresponding the combined factors
-		sharedBtreeClique new_clique = bayesTree.insert(bn, children);
+		BayesTree<Conditional> bayesTree(bn, children);
 
-		return make_pair(fg, new_clique);
+		return make_pair(fg, bayesTree);
 	}
 
 	/* ************************************************************************* */
 	template <class FG> template <class Conditional>
 	BayesTree<Conditional> JunctionTree<FG>::eliminate() {
-		BayesTree<Conditional> bayesTree;
+		pair<FG, BayesTree<Conditional> > ret = this->eliminateOneClique<Conditional>(root_);
-		eliminateOneClique(root_, bayesTree);
+//		ret.first.print("ret.first");
-		return bayesTree;
+		if (ret.first.nrFactors() != 0)
+			throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
+		return ret.second;
 	}
 
 	/* ************************************************************************* */
@@ -173,7 +181,8 @@ namespace gtsam {
 	VectorConfig GaussianJunctionTree<FG>::optimize() {
 		// eliminate from leaves to the root
 		typedef JunctionTree<FG> Base;
-		BayesTree<GaussianConditional> bayesTree = this->eliminate<GaussianConditional>();
+		BayesTree<GaussianConditional> bayesTree;
+				this->eliminate<GaussianConditional>();
 
 		// back-substitution
 		VectorConfig result;

cpp/JunctionTree.h

@@ -73,10 +73,9 @@ namespace gtsam {
 		// distribute the factors along the Bayes tree
 		sharedClique distributeFactors(FG& fg, const BayesTree<SymbolicConditional>::sharedClique clique);
 
-		// utility function called by eliminateBottomUp
+		// utility function called by eliminate
 		template <class Conditional>
-		std::pair<FG, typename BayesTree<Conditional>::sharedClique> eliminateOneClique(
+		std::pair<FG, BayesTree<Conditional> > eliminateOneClique(sharedClique fg_);
-				sharedClique fg_, BayesTree<Conditional>& bayesTree);
 
 	public:
 		// constructor
@@ -126,6 +125,6 @@ namespace gtsam {
 
 		// optimize the linear graph
 		VectorConfig optimize();
-	}; // Linear JunctionTree
+	}; // GaussianJunctionTree
 
 } // namespace gtsam

cpp/testGaussianFactor.cpp

@@ -809,6 +809,38 @@ TEST( GaussianFactor, erase)
 	CHECK(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactor, eliminateMatrix)
+{
+	Matrix Ab = Matrix_(3, 4,
+			1., 2., 0., 3.,
+			0., 4., 5., 6.,
+			0., 0., 7., 8.);
+	SharedDiagonal model(Vector_(3, 0.5, 0.5, 0.5));
+	Ordering frontals; frontals += "x1", "x2";
+	Ordering separator; separator += "x3";
+	Dimensions dimensions;
+	dimensions.insert(make_pair("x1", 1));
+	dimensions.insert(make_pair("x2", 1));
+	dimensions.insert(make_pair("x3", 1));
+
+	GaussianFactor::shared_ptr factor;
+	GaussianBayesNet bn;
+	boost::tie(bn, factor) =
+			GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions);
+
+	GaussianBayesNet bn_expected;
+	GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x1", Vector_(1, 6.), Matrix_(1, 1, 2.),
+			"x2", Matrix_(1, 1, 4.), "x3", Matrix_(1, 1, 0.), Vector_(1, 1.)));
+	GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x2", Vector_(1, 12.), Matrix_(1, 1, 8.),
+			"x3", Matrix_(1, 1, 10.), Vector_(1, 1.)));
+	bn_expected.push_back(conditional1);
+	bn_expected.push_back(conditional2);
+	CHECK(assert_equal(bn_expected, bn));
+
+	GaussianFactor factor_expected("x3", Matrix_(1, 1, 14.), Vector_(1, 16.), SharedDiagonal(Vector_(1, 1.)));
+	CHECK(assert_equal(factor_expected, *factor));
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

cpp/testGaussianFactorGraph.cpp

@@ -934,6 +934,42 @@ TEST ( GaussianFactorGraph, combine_matrix ) {
 	CHECK(assert_equal(*expModel, *noise));
 }
 
+/* ************************************************************************* */
+/**
+ *   x2 x1 x3 b
+ *    1  1    1       1  1  0  1
+ *    1    1  1  ->      1  1  1
+ *         1  1             1  1
+ */
+TEST ( GaussianFactorGraph, eliminateFrontals ) {
+	typedef GaussianFactorGraph::sharedFactor Factor;
+	SharedDiagonal model(Vector_(1, 0.5));
+	GaussianFactorGraph fg;
+	Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	fg.push_back(factor1);
+	fg.push_back(factor2);
+	fg.push_back(factor3);
+
+	Ordering frontals; frontals += "x2", "x1";
+	GaussianBayesNet bn = fg.eliminateFrontals(frontals);
+
+	GaussianBayesNet bn_expected;
+	GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x2", Vector_(1, 2.), Matrix_(1, 1, 2.),
+			"x1", Matrix_(1, 1, 1.), "x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
+	GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x1", Vector_(1, 0.), Matrix_(1, 1, -1.),
+			"x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
+	bn_expected.push_back(conditional1);
+	bn_expected.push_back(conditional2);
+	CHECK(assert_equal(bn_expected, bn));
+
+	GaussianFactorGraph::sharedFactor factor_expected(new GaussianFactor("x3", Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
+	GaussianFactorGraph fg_expected;
+	fg_expected.push_back(factor_expected);
+	CHECK(assert_equal(fg_expected, fg));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

cpp/testJunctionTree.cpp

@@ -23,6 +23,40 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+/* ************************************************************************* */
+/**
+ * x1 - x2 - x3 - x4
+ * x3 x4
+ *    x2 x1 : x3
+ */
+TEST( GaussianFactorGraph, constructor )
+{
+	typedef GaussianFactorGraph::sharedFactor Factor;
+	SharedDiagonal model(Vector_(1, 0.2));
+	Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.),  model));
+
+	GaussianFactorGraph fg;
+	fg.push_back(factor1);
+	fg.push_back(factor2);
+	fg.push_back(factor3);
+
+	Ordering ordering; ordering += "x2","x1","x3","x4";
+	GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
+
+	Ordering frontal1; frontal1 += "x3", "x4";
+	Ordering frontal2; frontal2 += "x2", "x1";
+	Unordered sep1;
+	Unordered sep2; sep2 += "x3";
+	CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
+	CHECK(assert_equal(sep1,     junctionTree.root()->separator()));
+	LONGS_EQUAL(1,               junctionTree.root()->size());
+	CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
+	CHECK(assert_equal(sep2,     junctionTree.root()->children()[0]->separator()));
+	LONGS_EQUAL(2,               junctionTree.root()->children()[0]->size());
+}
+
 /* ************************************************************************* *
  Bayes tree for smoother with "nested dissection" ordering:
 	 C1		 x5 x6 x4
@@ -30,7 +64,7 @@ using namespace example;
 	 C3		    x1 : x2
 	 C4		  x7 : x6
 /* ************************************************************************* */
-TEST( GaussianFactorGraph, constructor )
+TEST( GaussianFactorGraph, constructor2 )
 {
 	// create a graph
 	GaussianFactorGraph fg = createSmoother(7);
@@ -61,8 +95,51 @@ TEST( GaussianFactorGraph, constructor )
 	LONGS_EQUAL(2,               junctionTree.root()->children()[1]->size());
 }
 
-
 /* ************************************************************************* */
+/**
+ * x1 - x2 - x3 - x4
+ * x3 x4
+ *    x2 x1 : x3
+ *
+ * x2 x1 x3 x4  b
+ *  1  1        1
+ *  1     1     1
+ *        1  1  1
+ *           1  1
+ *
+ *  1  0  0  1
+ */
+TEST( GaussianFactorGraph, eliminate )
+{
+	typedef GaussianFactorGraph::sharedFactor Factor;
+	SharedDiagonal model(Vector_(1, 0.5));
+	Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.),  model));
+	Factor factor4(new GaussianFactor("x4", Matrix_(1,1,1.), Vector_(1,1.),  model));
+
+	GaussianFactorGraph fg;
+	fg.push_back(factor1);
+	fg.push_back(factor2);
+	fg.push_back(factor3);
+	fg.push_back(factor4);
+
+	Ordering ordering; ordering += "x2","x1","x3","x4";
+	GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
+		BayesTree<GaussianConditional> bayesTree = junctionTree.eliminate<GaussianConditional>();
+
+	typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
+	Matrix two = Matrix_(1,1,2.);
+	Matrix one = Matrix_(1,1,1.);
+	BayesTree<GaussianConditional> bayesTree_expected;
+	bayesTree_expected.insert(sharedConditional(new GaussianConditional("x4", Vector_(1,2.), two, Vector_(1,1.))), ordering);
+	bayesTree_expected.insert(sharedConditional(new GaussianConditional("x3", Vector_(1,2.), two, "x4", two, Vector_(1,1.))), ordering);
+	bayesTree_expected.insert(sharedConditional(new GaussianConditional("x1", Vector_(1,0.), one*(-1), "x3", one, Vector_(1,1.))), ordering);
+	bayesTree_expected.insert(sharedConditional(new GaussianConditional("x2", Vector_(1,2.), two, "x1", one, "x3", one, Vector_(1,1.))), ordering);
+	CHECK(assert_equal(bayesTree_expected, bayesTree));
+}
+
+/* ************************************************************************* *
 TEST( GaussianFactorGraph, optimizeMultiFrontal )
 {
 	// create a graph
@@ -72,10 +149,10 @@ TEST( GaussianFactorGraph, optimizeMultiFrontal )
 	Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
 
 	// optimize the graph
-//	GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
+	GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
-//	VectorConfig actual = junctionTree.optimize();
+	VectorConfig actual = junctionTree.optimize();
-//
+
-//	// verify
+	// verify
 //	VectorConfig expected = createCorrectDelta();
 //
 //  CHECK(assert_equal(expected,actual));