Revived generic eliminate and eliminateOne functions for partial elimination

gtsam/inference/FactorGraph.h

@@ -43,6 +43,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 	class FactorGraph {
 	public:
 	  typedef FACTOR FactorType;
+	  typedef typename FACTOR::KeyType KeyType;
 	  typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
 		typedef typename boost::shared_ptr<FACTOR> sharedFactor;
 		typedef typename std::vector<sharedFactor>::iterator iterator;
@@ -56,6 +57,11 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 	  /** typedef for an eliminate subroutine */
 	  typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
 
+	  /** Typedef for the result of factorization */
+	  typedef std::pair<
+	      boost::shared_ptr<typename FACTOR::ConditionalType>,
+	      FactorGraph<FACTOR> > FactorizationResult;
+
 	protected:
 
 	  /** concept check */

gtsam/inference/GenericSequentialSolver-inl.h

@@ -89,7 +89,7 @@ namespace gtsam {
 			const std::vector<Index>& js, Eliminate function) const {
 
 		// Compute a COLAMD permutation with the marginal variable constrained to the end.
-		Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
+		Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
 		Permutation::shared_ptr permutationInverse(permutation->inverse());
 
 		// Permute the factors - NOTE that this permutes the original factors, not

gtsam/inference/IndexConditional.cpp

@@ -31,12 +31,12 @@ namespace gtsam {
     // Checks for uniqueness of keys
     Base::assertInvariants();
 #ifndef NDEBUG
-    // Check that separator keys are sorted
+    // Check that frontal keys are sorted
-    FastSet<Index> uniquesorted(beginFrontals(), endFrontals());
+    //FastSet<Index> uniquesorted(beginFrontals(), endFrontals());
-    assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals()));
+    //assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals()));
-    // Check that separator keys are less than parent keys
+    //// Check that separator keys are less than parent keys
-    //BOOST_FOREACH(Index j, frontals()) {
+    ////BOOST_FOREACH(Index j, frontals()) {
-    //  assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); }
+    ////  assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); }
 #endif
   }
 
@@ -60,13 +60,13 @@ namespace gtsam {
   /* ************************************************************************* */
   void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
     // The permutation may not move the separators into the frontals
-  #ifndef NDEBUG
+//  #ifndef NDEBUG
-    BOOST_FOREACH(const KeyType frontal, this->frontals()) {
+//    BOOST_FOREACH(const KeyType frontal, this->frontals()) {
-      BOOST_FOREACH(const KeyType separator, this->parents()) {
+//      BOOST_FOREACH(const KeyType separator, this->parents()) {
-        assert(inversePermutation[frontal] < inversePermutation[separator]);
+//        assert(inversePermutation[frontal] < inversePermutation[separator]);
-      }
+//      }
-    }
+//    }
-  #endif
+//  #endif
 		BOOST_FOREACH(Index& key, keys())
 						key = inversePermutation[key];
     assertInvariants();

gtsam/inference/Permutation.h

@@ -26,8 +26,6 @@
 
 namespace gtsam {
 
-class Inference;
-
 /**
  * A permutation reorders variables, for example to reduce fill-in during
  * elimination.  To save computation, the permutation can be applied to
@@ -162,8 +160,6 @@ protected:
   void check(Index variable) const { assert(variable < rangeIndices_.size()); }
 
 	/// @}
-
-  friend class Inference;
 };
 
 

gtsam/inference/VariableIndex.h

@@ -26,8 +26,6 @@
 
 namespace gtsam {
 
-class Inference;
-
 /**
  * The VariableIndex class computes and stores the block column structure of a
  * factor graph.  The factor graph stores a collection of factors, each of 

gtsam/inference/inference-inl.h

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file inference-inl.h
+ * @brief 
+ * @author Richard Roberts
+ * @date Mar 3, 2012
+ */
+
+#pragma once
+
+#include <algorithm>
+
+// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h
+#include <gtsam/inference/inference.h>
+
+#include <gtsam/base/FastSet.h>
+
+namespace gtsam {
+
+namespace inference {
+
+/* ************************************************************************* */
+template<typename CONSTRAINED>
+Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
+
+  std::vector<int> cmember(variableIndex.size(), 0);
+
+  // If at least some variables are not constrained to be last, constrain the
+  // ones that should be constrained.
+  if(constrainLast.size() < variableIndex.size()) {
+    BOOST_FOREACH(Index var, constrainLast) {
+      assert(var < variableIndex.size());
+      cmember[var] = 1;
+    }
+  }
+
+  return PermutationCOLAMD_(variableIndex, cmember);
+}
+
+/* ************************************************************************* */
+inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) {
+  std::vector<int> cmember(variableIndex.size(), 0);
+  return PermutationCOLAMD_(variableIndex, cmember);
+}
+
+/* ************************************************************************* */
+template<class Graph>
+typename Graph::FactorizationResult eliminate(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);
+
+  // First find the involved factors
+  Graph involvedFactors;
+  Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors
+
+  // First get the indices of the involved factors, but uniquely in a set
+  FastSet<size_t> involvedFactorIndices;
+  BOOST_FOREACH(Index variable, variables) {
+    involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); }
+
+  // Add the factors themselves to involvedFactors and update largest index
+  involvedFactors.reserve(involvedFactorIndices.size());
+  BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) {
+    const typename Graph::sharedFactor factor = factorGraph[factorIndex];
+    involvedFactors.push_back(factor); // Add involved factor
+    highestInvolvedVariable = std::max( // Updated largest index
+        highestInvolvedVariable,
+        *std::max_element(factor->begin(), factor->end()));
+  }
+
+  // Now permute the variables to be eliminated to the front of the ordering
+  Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
+  Permutation toFrontInverse = *toFront.inverse();
+  involvedFactors.permuteWithInverse(toFrontInverse);
+
+  // Eliminate into conditional and remaining factor
+  typename Graph::EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
+  boost::shared_ptr<typename Graph::FactorType::ConditionalType> conditional = eliminated.first;
+  typename Graph::sharedFactor remainingFactor = eliminated.second;
+
+  // Undo the permutation
+  conditional->permuteWithInverse(toFront);
+  remainingFactor->permuteWithInverse(toFront);
+
+  // Build the remaining graph, without the removed factors
+  Graph remainingGraph;
+  remainingGraph.reserve(factorGraph.size() - involvedFactors.size() + 1);
+  FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
+  for(size_t i = 0; i < factorGraph.size(); ++i) {
+    if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i)
+      ++ involvedFactorIndexIt;
+    else
+      remainingGraph.push_back(factorGraph[i]);
+  }
+
+  // Add remaining factor
+  remainingGraph.push_back(remainingFactor);
+
+  return typename Graph::FactorizationResult(conditional, remainingGraph);
+
+}
+
+
+}
+
+}
+
+

gtsam/inference/inference.cpp

@@ -29,7 +29,9 @@ using namespace std;
 
 namespace gtsam {
 
-Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
+namespace inference {
+
+Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
   size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
   int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
@@ -79,10 +81,10 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia
   // Convert elimination ordering in p to an ordering
   Permutation::shared_ptr permutation(new Permutation(nVars));
   for (Index j = 0; j < nVars; j++) {
-//    if(p[j] == -1)
+    //    if(p[j] == -1)
-//      permutation->operator[](j) = j;
+    //      permutation->operator[](j) = j;
-//    else
+    //    else
-      permutation->operator[](j) = p[j];
+    permutation->operator[](j) = p[j];
     if(debug) cout << "COLAMD:  " << j << "->" << p[j] << endl;
   }
   if(debug) cout << "COLAMD:  p[" << nVars << "] = " << p[nVars] << endl;
@@ -91,3 +93,5 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia
 }
 
 }
+
+}

gtsam/inference/inference.h

@@ -22,58 +22,59 @@
 #include <gtsam/inference/Permutation.h>
 
 #include <boost/foreach.hpp>
+#include <boost/optional.hpp>
 
 #include <deque>
 
 namespace gtsam {
 
-	class Inference {
+namespace inference {
-	private:
-	  /* Static members only, private constructor */
-	  Inference() {}
 
-	public:
+/**
+ * Compute a permutation (variable ordering) using colamd
+ */
+Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
 
-    /**
+/**
-     * Compute a permutation (variable ordering) using colamd
+ * Compute a permutation (variable ordering) using constrained colamd
-     */
+ */
-    static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
+template<typename CONSTRAINED>
+Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
 
-    /**
+/**
-     * Compute a permutation (variable ordering) using constrained colamd
+ * Compute a CCOLAMD permutation using the constraint groups in cmember.
-     */
+ */
-    template<typename CONSTRAINED>
+Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
-    static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
 
-	  /**
+/** Factor the factor graph into a conditional and a remaining factor graph.
-	   * Compute a CCOLAMD permutation using the constraint groups in cmember.
+ * 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
-    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
+ * \f$ Y := X\V \f$ are the remaining variables.  If \f$ f(X) = p(X) \f$ is
+ * a probability density or likelihood, the factorization produces a
+ * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
+ *
+ * For efficiency, this function treats the variables to eliminate
+ * \c variables as fully-connected, so produces a dense (fully-connected)
+ * conditional on all of the variables in \c variables, instead of a sparse
+ * BayesNet.  If the variables are not fully-connected, it is more efficient
+ * to sequentially factorize multiple times.
+ */
+template<class Graph>
+typename Graph::FactorizationResult eliminate(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(const Graph&, const std::vector<typename Graph::KeyType>&, const typename Graph::Eliminate&, boost::optional<const VariableIndex&>)
+ */
+template<class Graph>
+typename Graph::FactorizationResult eliminateOne(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);
+}
 
-	/* ************************************************************************* */
+}
-	template<typename CONSTRAINED>
-	Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
-
-	  std::vector<int> cmember(variableIndex.size(), 0);
-
-	  // If at least some variables are not constrained to be last, constrain the
-	  // ones that should be constrained.
-	  if(constrainLast.size() < variableIndex.size()) {
-	    BOOST_FOREACH(Index var, constrainLast) {
-	      assert(var < variableIndex.size());
-	      cmember[var] = 1;
-	    }
-	  }
-
-	  return PermutationCOLAMD_(variableIndex, cmember);
-	}
-
-	/* ************************************************************************* */
-	inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
-	  std::vector<int> cmember(variableIndex.size(), 0);
-	  return PermutationCOLAMD_(variableIndex, cmember);
-	}
 
 } // namespace gtsam
+
+#include <gtsam/inference/inference-inl.h>

gtsam/inference/tests/testInference.cpp

@@ -25,7 +25,7 @@
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(Inference, UnobservedVariables) {
+TEST(inference, UnobservedVariables) {
   SymbolicFactorGraph sfg;
 
   // Create a factor graph that skips some variables
@@ -35,7 +35,7 @@ TEST(Inference, UnobservedVariables) {
 
   VariableIndex variableIndex(sfg);
 
-  Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex));
+  Permutation::shared_ptr colamd(inference::PermutationCOLAMD(variableIndex));
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -303,7 +303,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
       }
     }
   }
-  Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember));
+  Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember));
   toc(3,"ccolamd");
   tic(4,"ccolamd permutations");
   Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());

gtsam/nonlinear/ISAM2-inl.h

@@ -238,7 +238,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     if(theta_.size() > constrainedKeysSet.size()) {
       BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; }
     }
-    Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember));
+    Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
     Permutation::shared_ptr colamdInverse(colamd->inverse());
     toc(1,"CCOLAMD");
 

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -78,7 +78,7 @@ namespace gtsam {
 				"orderingCOLAMD: some variables in the graph are not constrained!");
 
 		// Compute a fill-reducing ordering with COLAMD
-		Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(
+		Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
 				variableIndex));
 
 		// Permute the Ordering with the COLAMD ordering

tests/testGaussianFactorGraph.cpp

@@ -192,96 +192,116 @@ TEST( GaussianFactorGraph, equals ) {
 //  EXPECT(assert_equal(expected,*actual));
 //}
 
-///* ************************************************************************* */
+/* ************************************************************************* */
-//TEST( GaussianFactorGraph, eliminateOne_x1 )
+TEST( GaussianFactorGraph, eliminateOne_x1 )
-//{
+{
-//  Ordering ordering; ordering += kx(1),kl(1),kx(2);
+  Ordering ordering; ordering += kx(1),kl(1),kx(2);
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
-//
-//  // 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[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
-//
-//  EXPECT(assert_equal(expected,*actual,tol));
-//}
-//
-///* ************************************************************************* */
-//TEST( GaussianFactorGraph, eliminateOne_x2 )
-//{
-//   Ordering ordering; ordering += kx(2),kl(1),kx(1);
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
-//
-//  // create expected Conditional Gaussian
-//  double sig = 0.0894427;
-//  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-//  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
-//  GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma);
-//
-//  EXPECT(assert_equal(expected,*actual,tol));
-//}
-//
-///* ************************************************************************* */
-//TEST( GaussianFactorGraph, eliminateOne_l1 )
-//{
-//  Ordering ordering; ordering += kl(1),kx(1),kx(2);
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
-//
-//  // create expected Conditional Gaussian
-//  double sig = sqrt(2)/10.;
-//  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
-//  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
-//  GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
-//
-//  EXPECT(assert_equal(expected,*actual,tol));
-//}
 
-///* ************************************************************************* */
+  GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR);
-//TEST( GaussianFactorGraph, eliminateOne_x1_fast )
+
-//{
+  // create expected Conditional Gaussian
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
+  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false);
+  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
-//
+  GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
-//  // create expected Conditional Gaussian
+
-//  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
+  EXPECT(assert_equal(expected,*result.first,tol));
-//  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
+}
-//  GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma);
+
-//
+/* ************************************************************************* */
-//  EXPECT(assert_equal(expected,*actual,tol));
+TEST( GaussianFactorGraph, eliminateOne_x2 )
-//}
+{
-//
+  Ordering ordering; ordering += kx(2),kl(1),kx(1);
-///* ************************************************************************* */
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//TEST( GaussianFactorGraph, eliminateOne_x2_fast )
+  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
-//{
+
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
+  // create expected Conditional Gaussian
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false);
+  double sig = 0.0894427;
-//
+  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-//  // create expected Conditional Gaussian
+  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
-//  double sig = 0.0894427;
+  GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma);
-//  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
+
-//  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
+  EXPECT(assert_equal(expected,*actual,tol));
-//  GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma);
+}
-//
+
-//  EXPECT(assert_equal(expected,*actual,tol));
+/* ************************************************************************* */
-//}
+TEST( GaussianFactorGraph, eliminateOne_l1 )
-//
+{
-///* ************************************************************************* */
+  Ordering ordering; ordering += kl(1),kx(1),kx(2);
-//TEST( GaussianFactorGraph, eliminateOne_l1_fast )
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//{
+  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
+
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false);
+  // create expected Conditional Gaussian
-//
+  double sig = sqrt(2)/10.;
-//  // create expected Conditional Gaussian
+  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
-//  double sig = sqrt(2)/10.;
+  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
-//  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
+  GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
-//  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
+
-//  GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma);
+  EXPECT(assert_equal(expected,*actual,tol));
-//
+}
-//  EXPECT(assert_equal(expected,*actual,tol));
+
-//}
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, eliminateOne_x1_fast )
+{
+  Ordering ordering; ordering += kx(1),kl(1),kx(2);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+  GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[kx(1)], EliminateQR);
+  GaussianConditional::shared_ptr conditional = result.first;
+  GaussianFactorGraph remaining = result.second;
+
+  // 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[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
+
+  // Create expected remaining new factor
+  JacobianFactor expectedFactor(1, Matrix_(4,2,
+             4.714045207910318,                   0.,
+                             0.,   4.714045207910318,
+                             0.,                   0.,
+                             0.,                   0.),
+     2, Matrix_(4,2,
+           -2.357022603955159,                   0.,
+                            0.,  -2.357022603955159,
+            7.071067811865475,                   0.,
+                            0.,   7.071067811865475),
+     Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), sharedUnit(4));
+
+  EXPECT(assert_equal(expected,*conditional,tol));
+  EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, eliminateOne_x2_fast )
+{
+  Ordering ordering; ordering += kx(1),kl(1),kx(2);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kx(2)], EliminateQR).first;
+
+  // create expected Conditional Gaussian
+  double sig = 0.0894427;
+  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
+  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
+  GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kx(1)],S13,ordering[kl(1)],S12,sigma);
+
+  EXPECT(assert_equal(expected,*actual,tol));
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, eliminateOne_l1_fast )
+{
+  Ordering ordering; ordering += kx(1),kl(1),kx(2);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first;
+
+  // create expected Conditional Gaussian
+  double sig = sqrt(2)/10.;
+  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
+  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
+  GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
+
+  EXPECT(assert_equal(expected,*actual,tol));
+}
 
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, eliminateAll )
@@ -439,7 +459,7 @@ TEST( GaussianFactorGraph, getOrdering)
 {
   Ordering original; original += kl(1),kx(1),kx(2);
   FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
-  Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic)));
+  Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
   Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
   Ordering expected; expected += kl(1),kx(2),kx(1);
   EXPECT(assert_equal(expected,actual));

tests/testGaussianISAM.cpp

@@ -85,7 +85,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
 //	Ordering ord; ord += kx(4),kx(3),kx(2),kx(1);
 //	GaussianFactorGraph factors1;
 //	for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
-//	GaussianISAM actual(*Inference::Eliminate(factors1));
+//	GaussianISAM actual(*inference::Eliminate(factors1));
 //
 //	// run iSAM with remaining factors
 //	GaussianFactorGraph factors2;
@@ -298,7 +298,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
 //	varIndex.permute(toFront);
 //	BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
 //	  factor->permuteWithInverse(toFrontInverse); }
-//	GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
+//	GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
 //	actual.permuteWithInverse(toFront);
 //	EXPECT(assert_equal(expected,actual,tol));
 //}

tests/testInference.cpp

@@ -30,7 +30,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 
 /* ************************************************************************* */
-TEST( Inference, marginals )
+TEST( inference, marginals )
 {
   using namespace example;
 	// create and marginalize a small Bayes net on "x"
@@ -45,7 +45,7 @@ TEST( Inference, marginals )
 }
 
 /* ************************************************************************* */
-TEST( Inference, marginals2)
+TEST( inference, marginals2)
 {
 	planarSLAM::Graph fg;
   SharedDiagonal poseModel(sharedSigma(3, 0.1));