Cleaned up typedefs in FactorGraph.h (and removed FactorizationResult), and also made sure ::shared_ptr was never assumed to exist for a FACTOR template argument. Should it exist, ever?

gtsam/inference/EliminationTree-inl.h

@@ -132,7 +132,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   // Hang factors in right places
   tic(3, "hang factors");
-  BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) {
+  BOOST_FOREACH(const typename boost::shared_ptr<DERIVEDFACTOR>& derivedFactor, factorGraph) {
     // Here we upwards-cast to the factor type of this EliminationTree.  This
     // allows performing symbolic elimination on, for example, GaussianFactors.
     if(derivedFactor) {

gtsam/inference/EliminationTree.h

@@ -52,7 +52,7 @@ public:
 
 	typedef EliminationTree<FACTOR> This; ///< This class
   typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
-  typedef typename FACTOR::shared_ptr sharedFactor; ///< Shared pointer to a factor
+	typedef typename boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
   typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
 
   /** Typedef for an eliminate subroutine */

gtsam/inference/FactorGraph-inl.h

@@ -38,6 +38,16 @@ namespace gtsam {
 
 	/* ************************************************************************* */
   template<class FACTOR>
+  template<class CONDITIONAL>
+  FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
+    factors_.reserve(bayesNet.size());
+    BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
+      this->push_back(cond->toFactor());
+    }
+  }
+
+	/* ************************************************************************* */
+	template<class FACTOR>
 	void FactorGraph<FACTOR>::print(const std::string& s) const {
 		std::cout << s << std::endl;
 		std::cout << "size: " << size() << std::endl;
@@ -50,7 +60,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class FACTOR>
-	bool FactorGraph<FACTOR>::equals(const FactorGraph<FACTOR>& fg, double tol) const {
+	bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
 		/** check whether the two factor graphs have the same number of factors_ */
 		if (factors_.size() != fg.size()) return false;
 
@@ -111,20 +121,20 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
+	// Recursive function to add factors in cliques to vector of factors_io
 	template<class FACTOR, class CONDITIONAL, class CLIQUE>
 	void _FactorGraph_BayesTree_adder(
-	    std::vector<typename FactorGraph<FACTOR>::sharedFactor>& factors,
+	    std::vector<typename boost::shared_ptr<FACTOR> >& factors_io,
 	    const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
 
 	  if(clique) {
 	    // Add factor from this clique
-	    factors.push_back((*clique)->toFactor());
+	  	factors_io.push_back((*clique)->toFactor());
 
 	    // Traverse children
 	    typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
-	    BOOST_FOREACH(const sharedClique& child, clique->children()) {
+	    BOOST_FOREACH(const sharedClique& child, clique->children())
-	      _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors, child);
+	      _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
-	    }
 	  }
 	}
 

gtsam/inference/FactorGraph.h

@@ -21,14 +21,13 @@
 
 #pragma once
 
-#include <boost/foreach.hpp>
-#include <boost/serialization/nvp.hpp>
-#include <boost/function.hpp>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/BayesNet.h>
 
+#include <boost/serialization/nvp.hpp>
+#include <boost/function.hpp>
+
 namespace gtsam {
 
 // Forward declarations
@@ -41,30 +40,28 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 	 */
 	template<class FACTOR>
 	class FactorGraph {
+
 	public:
-	  typedef FACTOR FactorType;
+
-	  typedef typename FACTOR::KeyType KeyType;
+		typedef FACTOR FactorType;  ///< factor type
-	  typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
+		typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index factors with
-		typedef typename boost::shared_ptr<FACTOR> sharedFactor;
+		typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
+		typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional;  ///< Shared pointer to a conditional
+
+		typedef FactorGraph<FACTOR> This;  ///< Typedef for this class
+		typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer for this class
 		typedef typename std::vector<sharedFactor>::iterator iterator;
 		typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
 
 		/** typedef for elimination result */
-	  typedef std::pair<
+		typedef std::pair<sharedConditional, sharedFactor> EliminationResult;
-				boost::shared_ptr<typename FACTOR::ConditionalType>,
-				typename FACTOR::shared_ptr> EliminationResult;
 
 		/** typedef for an eliminate subroutine */
-	  typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
+		typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
-
-	  /** Typedef for the result of factorization */
-	  typedef std::pair<
-	      boost::shared_ptr<typename FACTOR::ConditionalType>,
-	      FactorGraph<FACTOR> > FactorizationResult;
 
 	protected:
 
-	  /** concept check */
+	  /** concept check, makes sure FACTOR defines print and equals */
 	  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
 
     /** Collection of factors */
@@ -82,7 +79,11 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		/// @name Advanced Constructors
 		/// @{
 
-		/** convert from Bayes net */
+	  /**
+	   * @brief Constructor from a Bayes net
+	   * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
+	   * @return a factor graph with all the conditionals, as factors
+	   */
 		template<class CONDITIONAL>
 		FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
 
@@ -113,7 +114,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		}
 
 		/** push back many factors */
-		void push_back(const FactorGraph<FACTOR>& factors) {
+		void push_back(const This& factors) {
 			factors_.insert(end(), factors.begin(), factors.end());
 		}
 
@@ -123,9 +124,14 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 			factors_.insert(end(), firstFactor, lastFactor);
 		}
 
-    /** push back many factors stored in a vector*/
+	  /**
+	   * @brief Add a vector of derived factors
+	   * @param factors to add
+	   */
     template<typename DERIVEDFACTOR>
-    void push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors);
+    void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
+			factors_.insert(end(), factors.begin(), factors.end());
+		}
 
 		/// @}
   	/// @name Testable
@@ -135,7 +141,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		void print(const std::string& s = "FactorGraph") const;
 
 		/** Check equality */
-		bool equals(const FactorGraph<FACTOR>& fg, double tol = 1e-9) const;
+		bool equals(const This& fg, double tol = 1e-9) const;
 
 		/// @}
 		/// @name Standard Interface
@@ -252,31 +258,6 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 	template<class FACTORGRAPH>
 	FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
 
-	/*
-	 * These functions are defined here because they are templated on an
-	 * additional parameter.  Putting them in the -inl.h file would mean these
-	 * would have to be explicitly instantiated for any possible derived factor
-	 * type.
-	 */
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  template<class CONDITIONAL>
-  FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
-    factors_.reserve(bayesNet.size());
-    BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
-      this->push_back(cond->toFactor());
-    }
-  }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  template<class DERIVEDFACTOR>
-  void FactorGraph<FACTOR>::push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors) {
-    BOOST_FOREACH(const boost::shared_ptr<DERIVEDFACTOR>& factor, factors)
-      this->push_back(factor);
-  }
-
 } // namespace gtsam
 
 #include <gtsam/inference/FactorGraph-inl.h>

gtsam/inference/GenericMultifrontalSolver-inl.h

@@ -78,7 +78,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class F, class JT>
-	typename F::shared_ptr GenericMultifrontalSolver<F, JT>::marginalFactor(
+	typename boost::shared_ptr<F> GenericMultifrontalSolver<F, JT>::marginalFactor(
 			Index j, Eliminate function) const {
 		return eliminate(function)->marginalFactor(j, function);
 	}

gtsam/inference/GenericMultifrontalSolver.h

@@ -102,7 +102,7 @@ namespace gtsam {
 		 * Compute the marginal density over a variable, by integrating out
 		 * all of the other variables.  This function returns the result as a factor.
 		 */
-		typename FACTOR::shared_ptr marginalFactor(Index j,
+		typename boost::shared_ptr<FACTOR> marginalFactor(Index j,
 				Eliminate function) const;
 
 		/// @}

gtsam/inference/GenericSequentialSolver-inl.h

@@ -95,7 +95,7 @@ namespace gtsam {
 		// Permute the factors - NOTE that this permutes the original factors, not
 		// copies.  Other parts of the code may hold shared_ptr's to these factors so
 		// we must undo the permutation before returning.
-		BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
+		BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
 						if (factor) factor->permuteWithInverse(*permutationInverse);
 
 		// Eliminate all variables
@@ -103,7 +103,7 @@ namespace gtsam {
 			bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
 
 		// Undo the permuation on the original factors and on the structure.
-		BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
+		BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
 					if (factor) factor->permuteWithInverse(*permutation);
 
 		// Take the joint marginal from the Bayes net.
@@ -116,7 +116,7 @@ namespace gtsam {
 			joint->push_back((*(conditional++))->toFactor());
 
 		// Undo the permutation on the eliminated joint marginal factors
-		BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint)
+		BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *joint)
 						factor->permuteWithInverse(*permutation);
 
 		return joint;
@@ -124,7 +124,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class FACTOR>
-	typename FACTOR::shared_ptr //
+	typename boost::shared_ptr<FACTOR> //
 	GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
 		// Create a container for the one variable index
 		std::vector<Index> js(1);

gtsam/inference/GenericSequentialSolver.h

@@ -130,7 +130,7 @@ namespace gtsam {
 		 * Compute the marginal Gaussian density over a variable, by integrating out
 		 * all of the other variables.  This function returns the result as a factor.
 		 */
-		typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
+		typename boost::shared_ptr<FACTOR> marginalFactor(Index j, Eliminate function) const;
 
 		/// @}
 

gtsam/inference/inference-inl.h

@@ -31,7 +31,8 @@ namespace inference {
 
 /* ************************************************************************* */
 template<typename CONSTRAINED>
-Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
+Permutation::shared_ptr PermutationCOLAMD(
+		const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
 
   std::vector<int> cmember(variableIndex.size(), 0);
 
@@ -55,10 +56,14 @@ inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIn
 
 /* ************************************************************************* */
 template<class Graph>
-typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector<typename Graph::KeyType>& variables,
+std::pair<typename Graph::sharedConditional, Graph> eliminate(
-    const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex_) {
+		const Graph& factorGraph,
+		const std::vector<typename Graph::KeyType>& variables,
+		const typename Graph::Eliminate& eliminateFcn,
+		boost::optional<const VariableIndex&> variableIndex_) {
 
-  const VariableIndex& variableIndex = variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph);
+  const VariableIndex& variableIndex =
+					variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph);
 
   // First find the involved factors
   Graph involvedFactors;
@@ -108,13 +113,11 @@ typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const st
   if(remainingFactor->size() != 0)
   	remainingGraph.push_back(remainingFactor);
 
-  return typename Graph::FactorizationResult(conditional, remainingGraph);
+  return std::make_pair(conditional, remainingGraph);
 
-}
+} // eliminate
-
+
-
+} // namespace inference
-}
+} // namespace gtsam
-
-}
 
 

gtsam/inference/inference.h

@@ -29,25 +29,28 @@
 
 namespace gtsam {
 
-namespace inference {
+	namespace inference {
 
-/**
+		/**
 		 * Compute a permutation (variable ordering) using colamd
 		 */
-Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
+		Permutation::shared_ptr PermutationCOLAMD(
+				const VariableIndex& variableIndex);
 
-/**
+		/**
 		 * Compute a permutation (variable ordering) using constrained colamd
 		 */
-template<typename CONSTRAINED>
+		template<typename CONSTRAINED>
-Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
+		Permutation::shared_ptr PermutationCOLAMD(
+				const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
 
-/**
+		/**
 		 * Compute a CCOLAMD permutation using the constraint groups in cmember.
 		 */
-Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
+		Permutation::shared_ptr PermutationCOLAMD_(
+				const VariableIndex& variableIndex, std::vector<int>& cmember);
 
-/** Factor the factor graph into a conditional and a remaining factor graph.
+		/** Factor the factor graph into a conditional and a remaining factor graph.
 		 * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
 		 * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
 		 * \f$ Y := X\V \f$ are the remaining variables.  If \f$ f(X) = p(X) \f$ is
@@ -60,21 +63,26 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s
 		 * BayesNet.  If the variables are not fully-connected, it is more efficient
 		 * to sequentially factorize multiple times.
 		 */
-template<class Graph>
+		template<class Graph>
-typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector<typename Graph::KeyType>& variables,
+		std::pair<typename Graph::sharedConditional, Graph> eliminate(
-    const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex = boost::none);
+				const Graph& factorGraph,
+				const std::vector<typename Graph::KeyType>& variables,
+				const typename Graph::Eliminate& eliminateFcn,
+				boost::optional<const VariableIndex&> variableIndex = boost::none);
 
-/** Eliminate a single variable, by calling
+		/** Eliminate a single variable, by calling
 		 * eliminate(const Graph&, const std::vector<typename Graph::KeyType>&, const typename Graph::Eliminate&, boost::optional<const VariableIndex&>)
 		 */
-template<class Graph>
+		template<class Graph>
-typename Graph::FactorizationResult eliminateOne(const Graph& factorGraph, typename Graph::KeyType variable,
+		std::pair<typename Graph::sharedConditional, Graph> eliminateOne(
-    const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex = boost::none) {
+				const Graph& factorGraph, typename Graph::KeyType variable,
+				const typename Graph::Eliminate& eliminateFcn,
+				boost::optional<const VariableIndex&> variableIndex = boost::none) {
 			std::vector<size_t> variables(1, variable);
 			return eliminate(factorGraph, variables, eliminateFcn, variableIndex);
-}
+		}
 
-}
+	} // namespace inference
 
 } // namespace gtsam
 

gtsam/linear/GaussianFactorGraph.h

@@ -45,7 +45,7 @@ namespace gtsam {
   template<class FACTOR>
   boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
     boost::shared_ptr<Errors> e(new Errors);
-    BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
+    BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, fg) {
       e->push_back(factor->error_vector(x));
     }
     return e;

tests/testGaussianFactorGraphB.cpp

@@ -200,14 +200,16 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
   Ordering ordering; ordering += X(1),L(1),X(2);
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 
-  GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR);
+  GaussianConditional::shared_ptr conditional;
+  GaussianFactorGraph remaining;
+  boost::tie(conditional,remaining) = inference::eliminateOne(fg, 0, EliminateQR);
 
   // create expected Conditional Gaussian
   Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
   Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
   GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
 
-  EXPECT(assert_equal(expected,*result.first,tol));
+  EXPECT(assert_equal(expected,*conditional,tol));
 }
 
 /* ************************************************************************* */
@@ -247,9 +249,9 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
 {
   Ordering ordering; ordering += X(1),L(1),X(2);
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-  GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
+  GaussianConditional::shared_ptr conditional;
-  GaussianConditional::shared_ptr conditional = result.first;
+  GaussianFactorGraph remaining;
-  GaussianFactorGraph remaining = result.second;
+  boost::tie(conditional,remaining) = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
 
   // create expected Conditional Gaussian
   Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;