distribute the factors along the junction tree

.cproject

@@ -12,7 +12,8 @@
 					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@@ -301,7 +302,6 @@
 				<buildTargets>
 					<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>all</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -309,7 +309,6 @@
 					</target>
 					<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>clean</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -333,6 +332,7 @@
 					</target>
 					<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>clean</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -492,7 +492,6 @@
 					</target>
 					<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testBayesTree.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -500,6 +499,7 @@
 					</target>
 					<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testSymbolicBayesNet.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -507,7 +507,6 @@
 					</target>
 					<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -699,7 +698,6 @@
 					</target>
 					<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testGraph.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -755,6 +753,7 @@
 					</target>
 					<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testSimulated2D.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -802,7 +801,6 @@
 					</target>
 					<target name="testErrors.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testErrors.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -810,7 +808,6 @@
 					</target>
 					<target name="testDSF.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testDSF.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -826,7 +823,6 @@
 					</target>
 					<target name="testConstraintOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testConstraintOptimizer.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -834,6 +830,7 @@
 					</target>
 					<target name="testBTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testBTree.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -841,7 +838,6 @@
 					</target>
 					<target name="testSimulated2DOriented.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
-<buildArguments/>
 						<buildTarget>testSimulated2DOriented.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>false</useDefaultCommand>
@@ -849,6 +845,7 @@
 					</target>
 					<target name="testDSFVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testDSFVector.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -856,6 +853,7 @@
 					</target>
 					<target name="testSPQRUtil.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testSPQRUtil.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
@@ -863,11 +861,20 @@
 					</target>
 					<target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
+						<buildArguments/>
 						<buildTarget>testInference.run</buildTarget>
 						<stopOnError>true</stopOnError>
 						<useDefaultCommand>true</useDefaultCommand>
 						<runAllBuilders>true</runAllBuilders>
 					</target>
+					<target name="testJunctionTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+						<buildCommand>make</buildCommand>
+						<buildArguments/>
+						<buildTarget>testJunctionTree.run</buildTarget>
+						<stopOnError>true</stopOnError>
+						<useDefaultCommand>true</useDefaultCommand>
+						<runAllBuilders>true</runAllBuilders>
+					</target>
 					<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 						<buildCommand>make</buildCommand>
 						<buildArguments>-j2</buildArguments>

cpp/BayesTree.h

@@ -67,6 +67,9 @@ namespace gtsam {
 			/** is this the root of a Bayes tree ? */
 			inline bool isRoot() const { return parent_==NULL;}
 
+			/** return the const reference of children */
+			const std::list<shared_ptr>& children() const { return children_; }
+
 			/** The size of subtree rooted at this clique, i.e., nr of Cliques */
 			size_t treeSize() const;
 
@@ -230,9 +233,7 @@ namespace gtsam {
 		 * Altering Bayes trees
 		 */
 
-		/**
+		/** Remove all nodes */
-		 * Remove all nodes
-		 */
 		void clear();
 
 		/**

cpp/FactorGraph-inl.h

@@ -165,7 +165,7 @@ std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingleto
 		singletons.insert(new_singletons.begin(), new_singletons.end());
 
 		BOOST_FOREACH(const Symbol& singleton, new_singletons)
-			findAndRemoveFactors(singleton);
+			findAndRemoveFactors<vector<boost::shared_ptr<Factor> > >(singleton);
 
 		// exit when there are no more singletons
 		if (new_singletons.empty()) break;
@@ -317,9 +317,8 @@ list<int> FactorGraph<Factor>::factors(const Symbol& key) const {
 /* ************************************************************************* */
 /** find all non-NULL factors for a variable, then set factors to NULL       */
 /* ************************************************************************* */
-template<class Factor>
+template<class Factor> template<class Factors>
-vector<boost::shared_ptr<Factor> >
+Factors FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
-FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
 
 	// find all factor indices associated with the key
 	Indices::const_iterator it = indices_.find(key);
@@ -329,7 +328,7 @@ FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
 				+ (string)key + " not found");
 	const list<int>& factorsAssociatedWithKey = it->second;
 
-	vector<sharedFactor> found;
+	Factors found;
 	BOOST_FOREACH(const int& i, factorsAssociatedWithKey) {
 		sharedFactor& fi = factors_.at(i); // throws exception !
 		if(fi == NULL) continue; // skip NULL factors
@@ -477,7 +476,8 @@ std::pair<FactorGraph<Factor>, FactorGraph<Factor> > FactorGraph<Factor>::splitM
 template<class Factor> boost::shared_ptr<Factor>
 removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key)
 {
-	vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
+	typedef vector<boost::shared_ptr<Factor> > Factors;
+	Factors found = factorGraph.template findAndRemoveFactors<Factors>(key);
 	boost::shared_ptr<Factor> new_factor(new Factor(found));
 	return new_factor;
 }

cpp/FactorGraph.h

@@ -123,7 +123,8 @@ namespace gtsam {
      * from the factor graph
      * @param key the key for the given node
      */
-		std::vector<sharedFactor> findAndRemoveFactors(const Symbol& key);
+    template<class Factors>
+    Factors findAndRemoveFactors(const Symbol& key);
 
 		/**
 		 * find the minimum spanning tree using boost graph library

cpp/GaussianFactorGraph.cpp

@@ -16,6 +16,7 @@
 #include "FactorGraph-inl.h"
 #include "inference-inl.h"
 #include "iterative.h"
+#include "JunctionTree-inl.h"
 
 using namespace std;
 using namespace gtsam;
@@ -207,7 +208,8 @@ std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
 GaussianConditional::shared_ptr
 GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
 	// find and remove all factors connected to key
-	vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
+	typedef vector<GaussianFactor::shared_ptr> Factors;
+	Factors factors = findAndRemoveFactors<Factors>(key);
 
 	// Collect all dimensions as well as the set of separator keys
 	set<Symbol> separator;
@@ -285,6 +287,14 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
 	return delta;
 }
 
+/* ************************************************************************* */
+VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
+{
+	GaussianJunctionTree<GaussianFactorGraph> junctionTree(*this, ordering);
+
+	return junctionTree.optimize();
+}
+
 /* ************************************************************************* */
 boost::shared_ptr<GaussianBayesNet>
 GaussianFactorGraph::eliminate_(const Ordering& ordering)

cpp/GaussianFactorGraph.h

@@ -154,6 +154,11 @@ namespace gtsam {
      */
     VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true);
 
+    /**
+     * optimize a linear factor graph with multi-frontals
+     */
+    VectorConfig optimizeMultiFrontals(const Ordering& ordering);
+
     /**
      * shared pointer versions for MATLAB
      */

cpp/JunctionTree-inl.h

@@ -8,43 +8,113 @@
 
 #pragma once
 
-#include <boost/tuple/tuple.hpp>
 #include <boost/foreach.hpp>
-#include <boost/optional.hpp>
+
-#include "Pose2.h"
-#include "BayesTree-inl.h"
 #include "SymbolicFactorGraph.h"
-
+#include "BayesTree-inl.h"
 #include "JunctionTree.h"
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cout
-
 namespace gtsam {
 
 	using namespace std;
 
+	/* ************************************************************************* */
+	template <class FG>
+	bool JunctionTree<FG>::Clique::equals(const JunctionTree<FG>::Clique& other) const {
+		if (!frontal_.equals(other.frontal_))
+			return false;
+
+		if (!separator_.equals(other.separator_))
+			return false;
+
+		if (children_.size() != other.children_.size())
+			return false;
+
+		typename vector<shared_ptr>::const_iterator it1 = children_.begin();
+		typename vector<shared_ptr>::const_iterator it2 = other.children_.begin();
+		for(; it1!=children_.end(); it1++, it2++)
+			if (!(*it1)->equals(**it2)) return false;
+
+		return true;
+	}
+
 	/* ************************************************************************* */
 	/**
-	 * Linear JunctionTree
+	 * JunctionTree
 	 */
-	template <class Conditional, class FG>
+	template <class FG>
-	void JunctionTree<Conditional, FG>::SubFG::printTree(const string& indent) const {
+	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 << " ";
+		cout << ":";
+		BOOST_FOREACH(const Symbol& key, separator_)
+		cout << (string)key << " ";
+		cout << endl;
+	}
+
+	/* ************************************************************************* */
+	template <class FG>
+	void JunctionTree<FG>::Clique::printTree(const string& indent) const {
 		print(indent);
 		BOOST_FOREACH(const shared_ptr& child, children_)
 			child->printTree(indent+"  ");
 	}
 
 	/* ************************************************************************* */
-	template <class Conditional, class FG>
+	template <class FG>
+	JunctionTree<FG>::JunctionTree(FG& fg, const Ordering& ordering) {
+		// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree
+		SymbolicFactorGraph sfg(fg);
+		SymbolicBayesNet sbn = sfg.eliminate(ordering);
+		BayesTree<SymbolicConditional> sbt(sbn);
+		sbt.print("sbt");
+
+		// distribtue factors
+		root_ = distributeFactors(fg, sbt.root());
+	}
+
+	/* ************************************************************************* */
+	template <class FG>
+	typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(FG& fg,
+			const BayesTree<SymbolicConditional>::sharedClique bayesClique) {
+		// create a new clique in the junction tree
+		sharedClique clique(new Clique());
+		clique->frontal_ = bayesClique->ordering();
+		clique->separator_.insert(bayesClique->separator_.begin(), bayesClique->separator_.end());
+
+		// recursively call the children
+		BOOST_FOREACH(const BayesTree<SymbolicConditional>::sharedClique bayesChild, bayesClique->children()) {
+			sharedClique child = distributeFactors(fg, bayesChild);
+			clique->children_.push_back(child);
+			child->parent_ = clique;
+		}
+
+		// collect the factors
+		typedef vector<typename FG::sharedFactor> Factors;
+		BOOST_FOREACH(const Symbol& frontal, clique->frontal_) {
+			Factors factors = fg.template findAndRemoveFactors<Factors>(frontal);
+			BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) {
+				clique->push_back(factor_);
+			}
+		}
+
+		return clique;
+	}
+
+	/* ************************************************************************* */
+	template <class FG> template <class Conditional>
 	pair<FG, typename BayesTree<Conditional>::sharedClique>
-	JunctionTree<Conditional, FG>::eliminateOneClique(sharedSubFG current, BayesTree<Conditional>& bayesTree) {
+	JunctionTree<FG>::eliminateOneClique(sharedClique current, BayesTree<Conditional>& bayesTree) {
+		typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
 		FG fg; // factor graph will be assembled from local factors and marginalized children
-		list<sharedClique> children;
+		list<sharedBtreeClique> children;
 		fg.push_back(*current); // add the local factor graph
-		BOOST_FOREACH(sharedSubFG& child, current->children_) {
+		BOOST_FOREACH(sharedClique& child, current->children_) {
 			// receive the factors from the child and its clique point
-			FG fgChild; sharedClique cliqueChild;
+			FG fgChild; sharedBtreeClique cliqueChild;
 			boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree);
 			if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!");
 
@@ -58,57 +128,54 @@ namespace gtsam {
 		BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
 
 		// create a new clique corresponding the combined factors
-		sharedClique new_clique = bayesTree.insert(bn, children);
+		sharedBtreeClique new_clique = bayesTree.insert(bn, children);
 
 		return make_pair(fg, new_clique);
 	}
 
 	/* ************************************************************************* */
-	template <class Conditional, class FG>
+	template <class FG> template <class Conditional>
-	BayesTree<Conditional> JunctionTree<Conditional, FG>::eliminate() {
+	BayesTree<Conditional> JunctionTree<FG>::eliminate() {
 		BayesTree<Conditional> bayesTree;
-		eliminateOneClique(rootFG_, bayesTree);
+		eliminateOneClique(root_, bayesTree);
 		return bayesTree;
 	}
 
 	/* ************************************************************************* */
-	template <class Conditional, class FG>
+	template <class FG>
-	void JunctionTree<Conditional, FG>::iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current) {
+	bool JunctionTree<FG>::equals(const JunctionTree<FG>& other, double tol) const {
-		if (!current.get()) current = rootFG_;
+		if (!root_ || !other.root_) return false;
-//		iterateBFS<SubFG>(visitor, current);
+		return root_->equals(*other.root_);
-	}
-
-	/* ************************************************************************* */
-	template <class Conditional, class FG>
-	void JunctionTree<Conditional, FG>::iterSubGraphsBFS(VisitorSubFG visitor) {
-//		iterateDFS<SubFG>(visitor, rootFG_);
 	}
 
 	/* ************************************************************************* */
 	/**
-	 * Linear JunctionTree
+	 * GaussianJunctionTree
 	 */
-	template <class Conditional, class FG>
+	template <class FG>
-	void LinearJunctionTree<Conditional, FG>::btreeBackSubstitue(typename BayesTree<Conditional>::sharedClique current, VectorConfig& config) {
+	void GaussianJunctionTree<FG>::btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config) {
 		// solve the bayes net in the current node
-		typename BayesNet<Conditional>::const_reverse_iterator it = current->rbegin();
+		typename BayesNet<GaussianConditional>::const_reverse_iterator it = current->rbegin();
 		for (; it!=current->rend(); it++) {
 			Vector x = (*it)->solve(config); // Solve for that variable
 			config.insert((*it)->key(),x);   // store result in partial solution
 		}
 
 		// solve the bayes nets in the child nodes
-		BOOST_FOREACH(sharedClique child, current->children_) {
+		typedef typename BayesTree<GaussianConditional>::sharedClique sharedBayesClique;
+		BOOST_FOREACH(sharedBayesClique child, current->children_) {
 			btreeBackSubstitue(child, config);
 		}
 	}
 
 	/* ************************************************************************* */
-	template <class Conditional, class FG>
+	template <class FG>
-	VectorConfig LinearJunctionTree<Conditional, FG>::optimize() {
+	VectorConfig GaussianJunctionTree<FG>::optimize() {
 		// eliminate from leaves to the root
-		BayesTree<Conditional> bayesTree = JunctionTree<Conditional, FG>::eliminate();
+		typedef JunctionTree<FG> Base;
+		BayesTree<GaussianConditional> bayesTree = this->eliminate<GaussianConditional>();
 
+		// back-substitution
 		VectorConfig result;
 		btreeBackSubstitue(bayesTree.root(), result);
 		return result;

cpp/JunctionTree.h

@@ -18,128 +18,114 @@
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template <class Conditional, class FG>
+	template <class FG>
-	class JunctionTree/*: public BayesTree<Conditional>*/ {
+	class JunctionTree : public Testable<JunctionTree<FG> > {
 	public:
-		typedef typename BayesTree<Conditional>::sharedClique sharedClique;
-
-		// the threshold for the sizes of submaps. Smaller ones will be absorbed into the separator
-		static const int const_minNodesPerMap_default = 10;
-		static const int const_minNodesPerMap_ultra = 1;
-
-		// when to stop partitioning
-		static const int const_numNodeStopPartition_default = 50;
-		static const int const_numNodeStopPartition_ultra = 3;     // so that A,B,C all have one variable
-
-
 		// the class for subgraphs that also include the pointers to the parents and two children
-		class SubFG : public FG {
+		class Clique : public FG {
-		public:
+		private:
-			typedef typename boost::shared_ptr<SubFG> shared_ptr;
+			typedef typename boost::shared_ptr<Clique> shared_ptr;
 			shared_ptr parent_;                  // the parent subgraph node
+			std::vector<shared_ptr> children_;   // the child cliques
 			Ordering frontal_;                   // the frontal varaibles
 			Unordered separator_;                // the separator variables
 
-			friend class JunctionTree<Conditional, FG>;
+			friend class JunctionTree<FG>;
 
 		public:
-			std::vector<shared_ptr> children_;         // the child cliques
 
 			// empty constructor
-			SubFG() {}
+			Clique() {}
 
 			// constructor with all the information
-			SubFG(const FG& fgLocal, const Ordering& frontal, const Unordered& separator,
+			Clique(const FG& fgLocal, const Ordering& frontal, const Unordered& separator,
 					 const shared_ptr& parent)
 				: frontal_(frontal), separator_(separator), FG(fgLocal), parent_(parent) {}
 
 			// constructor for an empty graph
-			SubFG(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent)
+			Clique(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent)
 				: frontal_(frontal), separator_(separator), parent_(parent) {}
 
+			// return the members
 			const Ordering& frontal() const            { return frontal_;}
 			const Unordered& separator() const         { return separator_;}
-			std::vector<shared_ptr>& children()        { return children_; } // TODO:: add const
+			const std::vector<shared_ptr>& children()  { return children_; }
 
 			// add a child node
 			void addChild(const shared_ptr& child) { children_.push_back(child); }
 
+			// print the object
+			void print(const std::string& indent) const;
 			void printTree(const std::string& indent) const;
+
+			// check equality
+			bool equals(const Clique& other) const;
 		};
 
 		// typedef for shared pointers to cliques
-		typedef typename SubFG::shared_ptr sharedSubFG;
+		typedef typename Clique::shared_ptr sharedClique;
-		typedef boost::function<void (sharedSubFG)> VisitorSubFG;
 
 	protected:
 		// Root clique
-		sharedSubFG rootFG_;
+		sharedClique root_;
 
 	private:
+		// distribute the factors along the Bayes tree
+		sharedClique distributeFactors(FG& fg, const BayesTree<SymbolicConditional>::sharedClique clique);
 
 		// utility function called by eliminateBottomUp
-		std::pair<FG, sharedClique> eliminateOneClique(sharedSubFG fg_, BayesTree<Conditional>& bayesTree);
+		template <class Conditional>
+		std::pair<FG, typename BayesTree<Conditional>::sharedClique> eliminateOneClique(
+				sharedClique fg_, BayesTree<Conditional>& bayesTree);
 
 	public:
+		// constructor
+		JunctionTree() {}
 
-		JunctionTree() : verboseLevel(0) {}
+		// constructor given a factor graph and the elimination ordering
+		JunctionTree(FG& fg, const Ordering& ordering);
 
-		// return the root graph
+		// return the root clique
-		sharedSubFG rootFG() const { return rootFG_; }
+		sharedClique root() const { return root_; }
 
 		// eliminate the factors in the subgraphs
+		template <class Conditional>
 		BayesTree<Conditional> eliminate();
 
 		// print the object
 		void print(const std::string& str) const {
-			if (rootFG_.get()) rootFG_->printTree("");
+			cout << str << endl;
+			if (root_.get()) root_->printTree("");
 		}
 
-		// iterate over all the subgraphs from root to leaves in the DFS order, recursive
+		/** check equality */
-		void iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current = sharedSubFG());
+		bool equals(const JunctionTree<FG>& other, double tol = 1e-9) const;
 
-		// iterate over all the subgraphs from root to leaves in the BFS order, non-recursive
-		void iterSubGraphsBFS(VisitorSubFG visitor);
-
-		// the output level
-		int verboseLevel;
 	}; // JunctionTree
 
 	/* ************************************************************************* */
 	/**
-	 * Linear JunctionTree which can do optimization
+	 * GaussianJunctionTree that does the optimization
 	 */
-	template <class Conditional, class FG>
+	template <class FG>
-	class LinearJunctionTree: public JunctionTree<Conditional, FG> {
+	class GaussianJunctionTree: public JunctionTree<FG> {
 	public:
-		typedef JunctionTree<Conditional, FG> Base;
+		typedef JunctionTree<FG> Base;
-		typedef typename BayesTree<Conditional>::sharedClique sharedClique;
+		typedef typename JunctionTree<FG>::sharedClique sharedClique;
-		typedef typename JunctionTree<Conditional, FG>::sharedSubFG sharedSubFG;
 
 	protected:
 		// back-substitute in topological sort order (parents first)
-		void btreeBackSubstitue(typename BayesTree<Conditional>::sharedClique current, VectorConfig& config);
+		void btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config);
 
 	public :
 
-		LinearJunctionTree() : Base() {}
+		GaussianJunctionTree() : Base() {}
 
 		// constructor
-		LinearJunctionTree(const FG& fg, const Ordering& ordering, int numNodeStopPartition = Base::const_numNodeStopPartition_default,
+		GaussianJunctionTree(FG& fg, const Ordering& ordering) : Base(fg, ordering) {}
-				int minNodesPerMap = Base::const_minNodesPerMap_default) :
-			Base(fg, ordering, numNodeStopPartition, minNodesPerMap) {}
 
 		// optimize the linear graph
 		VectorConfig optimize();
 	}; // Linear JunctionTree
 
-	class SymbolicConditional;
-	class SymbolicFactorGraph;
-
-	/**
-	 *  recursive partitioning
-	 */
-	typedef JunctionTree<SymbolicConditional, SymbolicFactorGraph> SymbolicTSAM;
-	typedef JunctionTree<GaussianConditional, GaussianFactorGraph> GaussianTSAM;
-
 } // namespace gtsam

cpp/Makefile.am

@@ -75,13 +75,15 @@ headers += BayesTree.h BayesTree-inl.h
 headers += ISAM.h ISAM-inl.h GaussianISAM.h
 headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h
 sources += GaussianISAM.cpp GaussianISAM2.cpp
-check_PROGRAMS += testGraph testFactorGraph testInference testOrdering
+check_PROGRAMS += testGraph testFactorGraph testJunctionTree testInference testOrdering
 check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
 testGraph_SOURCES              = testGraph.cpp
 testGraph_LDADD                = libgtsam.la
 testFactorGraph_SOURCES        = testFactorgraph.cpp
-testInference_SOURCES          = testInference.cpp
 testFactorGraph_LDADD          = libgtsam.la
+testJunctionTree_SOURCES       = testJunctionTree.cpp
+testJunctionTree_LDADD         = libgtsam.la
+testInference_SOURCES          = testInference.cpp
 testInference_LDADD            = libgtsam.la
 testBayesTree_SOURCES          = testBayesTree.cpp
 testBayesTree_LDADD            = libgtsam.la

cpp/Matrix.cpp

@@ -693,7 +693,7 @@ void householder(Matrix &A) {
 	__CLPK_integer info;
 
 	dgeqrf_(&m, &n, a, &m, tau, &work_optimal_size, &lwork, &info);
-	lwork = (int)work_optimal_size;
+	lwork = (__CLPK_integer)work_optimal_size;
 	double work[lwork];
 	dgeqrf_(&m, &n, a, &m, tau, work, &lwork, &info);
 	int k0 = 0;

cpp/testGaussianFactorGraph.cpp

@@ -475,38 +475,6 @@ TEST( GaussianFactorGraph, optimize )
   CHECK(assert_equal(expected,actual));
 }
 
-/* ************************************************************************* *
- Bayes tree for smoother with "nested dissection" ordering:
-	 C1		 x5 x6 x4
-	 C2		  x3 x2 : x4
-	 C3		    x1 : x2
-	 C4		  x7 : x6
-
-/* ************************************************************************* */
-TEST( GaussianFactorGraph, optimizeMultiFrontal )
-{
-	// create a graph
-	GaussianFactorGraph fg = createSmoother(7);
-
-	// create an ordering
-	Ordering ordering;
-	ordering += "x1","x3","x5","x7","x2","x6","x4";
-
-	// Symbolic factorization
-	// GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree
-	SymbolicFactorGraph sfg(fg);
-	SymbolicBayesNet sbn = sfg.eliminate(ordering);
-	BayesTree<SymbolicConditional> sbt(sbn);
-
-//	// optimize the graph
-//	VectorConfig actual = fg.optimizeMultiFrontal(sbt);
-//
-//	// verify
-//	VectorConfig expected = createCorrectDelta();
-//
-//  CHECK(assert_equal(expected,actual));
-}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, combine)
 {
@@ -584,7 +552,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors )
   GaussianFactor::shared_ptr f2 = fg[2];
 
   // call the function
-  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
+  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors
+  		<vector<GaussianFactor::shared_ptr> >("x1");
 
   // Check the factors
   CHECK(f0==factors[0]);
@@ -607,7 +576,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors_twice )
   GaussianFactor::shared_ptr f2 = fg[2];
 
   // call the function
-  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
+  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors
+  		<vector<GaussianFactor::shared_ptr> >("x1");
 
   // Check the factors
   CHECK(f0==factors[0]);

cpp/testJunctionTree.cpp

@@ -0,0 +1,86 @@
+/*
+ * testJunctionTree.cpp
+ *
+ *   Created on: Jul 8, 2010
+ *       Author: nikai
+ *  Description:
+ */
+
+#include <iostream>
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/set.hpp> // for operator +=
+using namespace boost::assign;
+
+#define GTSAM_MAGIC_KEY
+
+#include "smallExample.h"
+#include "JunctionTree-inl.h"
+
+using namespace std;
+using namespace gtsam;
+using namespace example;
+
+/* ************************************************************************* *
+ Bayes tree for smoother with "nested dissection" ordering:
+	 C1		 x5 x6 x4
+	 C2		  x3 x2 : x4
+	 C3		    x1 : x2
+	 C4		  x7 : x6
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, constructor )
+{
+	// create a graph
+	GaussianFactorGraph fg = createSmoother(7);
+
+	// create an ordering
+	Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
+
+	GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
+	Ordering frontal1; frontal1 += "x5", "x6", "x4";
+	Ordering frontal2; frontal2 += "x3", "x2";
+	Ordering frontal3; frontal3 += "x1";
+	Ordering frontal4; frontal4 += "x7";
+	Unordered sep1;
+	Unordered sep2; sep2 += "x4";
+	Unordered sep3; sep3 += "x2";
+	Unordered sep4; sep4 += "x6";
+	CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
+	CHECK(assert_equal(sep1,     junctionTree.root()->separator()));
+	LONGS_EQUAL(5,               junctionTree.root()->size());
+	CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
+	CHECK(assert_equal(sep2,     junctionTree.root()->children()[0]->separator()));
+	LONGS_EQUAL(4,               junctionTree.root()->children()[0]->size());
+	CHECK(assert_equal(frontal3, junctionTree.root()->children()[0]->children()[0]->frontal()));
+	CHECK(assert_equal(sep3,     junctionTree.root()->children()[0]->children()[0]->separator()));
+	LONGS_EQUAL(2,               junctionTree.root()->children()[0]->children()[0]->size());
+	CHECK(assert_equal(frontal4, junctionTree.root()->children()[1]->frontal()));
+	CHECK(assert_equal(sep4,     junctionTree.root()->children()[1]->separator()));
+	LONGS_EQUAL(2,               junctionTree.root()->children()[1]->size());
+}
+
+
+/* ************************************************************************* *
+TEST( GaussianFactorGraph, optimizeMultiFrontal )
+{
+	// create a graph
+	GaussianFactorGraph fg = createSmoother(7);
+
+	// create an ordering
+	Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
+
+	// optimize the graph
+	LinearJunctionTree<GaussianConditional, GaussianFactorGraph> junctionTree(fg, ordering);
+	VectorConfig actual = junctionTree.optimize();
+
+	// verify
+	VectorConfig expected = createCorrectDelta();
+
+  CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */

cpp/testSymbolicFactorGraph.cpp

@@ -46,7 +46,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors )
 	SymbolicFactorGraph actual(factorGraph);
   SymbolicFactor::shared_ptr f1 = actual[0];
   SymbolicFactor::shared_ptr f3 = actual[2];
-	actual.findAndRemoveFactors("x2");
+	actual.findAndRemoveFactors<SymbolicFactorGraph>("x2");
 
 	// construct expected graph after find_factors_and_remove
 	SymbolicFactorGraph expected;