Re-enabled computing marginals of a set of variables (Inference::Marg…

…inal(...))

.project

@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j2</value>
+					<value></value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>

inference/BayesNet.h

@@ -102,8 +102,14 @@ namespace gtsam {
 		/** SLOW O(n) random access to Conditional by key */
 		sharedConditional operator[](varid_t key) const;
 
+    /** return last node in ordering */
+    sharedConditional& front() { return conditionals_.front(); }
+
+    /** return last node in ordering */
+    boost::shared_ptr<const Conditional> front() const { return conditionals_.front(); }
+
 		/** return last node in ordering */
-		inline sharedConditional& back() { return conditionals_.back(); }
+		sharedConditional& back() { return conditionals_.back(); }
 
 		/** return last node in ordering */
 		boost::shared_ptr<const Conditional> back() const { return conditionals_.back(); }

inference/inference-inl.h

@@ -276,6 +276,41 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
   }
 }
 
+/* ************************************************************************* */
+template<class FactorGraph, class VarContainer>
+typename FactorGraph::bayesnet_type::shared_ptr Inference::Marginal(const FactorGraph& factorGraph, const VarContainer& variables) {
+
+  // Compute a COLAMD permutation with the marginal variables constrained to the end
+  typename FactorGraph::variableindex_type varIndex(factorGraph);
+  Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(varIndex, variables));
+  Permutation::shared_ptr permutationInverse(permutation->inverse());
+
+  // Copy and permute the factors
+  varIndex.permute(*permutation);
+  FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size());
+  BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
+    typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::factor_type(*factor));
+    permFactor->permuteWithInverse(*permutationInverse);
+    eliminationGraph.push_back(permFactor);
+  }
+
+  // Eliminate all variables
+  typename FactorGraph::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex));
+
+  // The last conditionals in the eliminated BayesNet contain the marginal for
+  // the variables we want.
+  typename FactorGraph::bayesnet_type::shared_ptr marginal(new typename FactorGraph::bayesnet_type());
+  typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
+  for(varid_t j=0; j<variables.size(); ++j, ++conditional) {
+    marginal->push_front(*conditional);
+    assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end());
+  }
+
+  // Undo the permutation
+  marginal->permuteWithInverse(*permutation);
+  return marginal;
+}
+
 /* ************************************************************************* */
 template<class VariableIndexType, typename ConstraintContainer>
 Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast) {

inference/inference.h

@@ -85,6 +85,17 @@ class Conditional;
     static boost::shared_ptr<Conditional>
     EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, varid_t var);
 
+    /**
+     * Eliminate all variables except the specified ones.  Internally this
+     * permutes these variables to the end of the ordering, eliminates all
+     * other variables, and then undoes the permutation.  This is
+     * inefficient if multiple marginals are needed - in that case use the
+     * BayesTree which supports efficiently computing marginals for multiple
+     * variables.
+     */
+    template<class FactorGraph, class VarContainer>
+    static typename FactorGraph::bayesnet_type::shared_ptr Marginal(const FactorGraph& factorGraph, const VarContainer& variables);
+
     /**
      * Compute a permutation (variable ordering) using colamd
      */

myconfigure

@@ -1,4 +1,4 @@
-../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
+../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CPPFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
 #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
 #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
 #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack

myconfigure.profiling

@@ -1,3 +1,3 @@
 #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
 #../configure --disable-fast-install --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g " --disable-static --enable-blas --enable-lapack
-../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-g -DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
+../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPPFLAGS="-g -DNDEBUG -O3" LDFLAGS="-g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack

myconfigure.timing

@@ -1,2 +1,2 @@
-../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
+../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
 #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CXXFLAGS="-DNDEBUG -g" CFLAGS="-DNDEBUG -g" LDFLAGS="-DNDEBUG -g" --disable-static --enable-blas --enable-lapack

slam/smallExample.cpp

@@ -171,8 +171,8 @@ namespace example {
 		tau(0) = 1.0;
 
 		// define nodes and specify in reverse topological sort (i.e. parents last)
-		GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11,
-				_y_, S12, tau)), Py(new GaussianConditional(_y_, d2, R22, tau));
+		GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
+		GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
 		GaussianBayesNet cbn;
 		cbn.push_back(Px_y);
 		cbn.push_back(Py);

tests/Makefile.am

@@ -12,7 +12,7 @@ check_PROGRAMS =
 check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGraph
 check_PROGRAMS += testGaussianISAM 
 check_PROGRAMS += testGraph
-#check_PROGRAMS += testInference
+check_PROGRAMS += testInference
 #check_PROGRAMS += testIterative
 check_PROGRAMS += testGaussianJunctionTree 
 check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph

tests/testInference.cpp

@@ -23,30 +23,29 @@ using namespace example;
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, createSmoother)
 {
-	GaussianFactorGraph fg2 = createSmoother(3);
+	GaussianFactorGraph fg2;
+	Ordering ordering;
+	boost::tie(fg2,ordering) = createSmoother(3);
 	LONGS_EQUAL(5,fg2.size());
 
 	// eliminate
-	Ordering ordering;
-	GaussianBayesNet bayesNet = fg2.eliminate(ordering);
-	FactorGraph<GaussianFactor> p_x3 = marginalize<GaussianFactor,GaussianConditional>(bayesNet, Ordering("x3"));
-	FactorGraph<GaussianFactor> p_x1 = marginalize<GaussianFactor,GaussianConditional>(bayesNet, Ordering("x1"));
-	CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry
+	list<varid_t> x3var; x3var.push_back(ordering["x3"]);
+  list<varid_t> x1var; x1var.push_back(ordering["x1"]);
+	GaussianBayesNet p_x3 = *Inference::Marginal(fg2, x3var);
+	GaussianBayesNet p_x1 = *Inference::Marginal(fg2, x1var);
+	CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
 }
 
 /* ************************************************************************* */
 TEST( Inference, marginals )
 {
 	// create and marginalize a small Bayes net on "x"
   GaussianBayesNet cbn = createSmallGaussianBayesNet();
-  Ordering keys("x");
-  FactorGraph<GaussianFactor> fg = marginalize<GaussianFactor, GaussianConditional>(cbn,keys);
-
-  // turn into Bayes net to test easily
-  BayesNet<GaussianConditional> actual = eliminate<GaussianFactor,GaussianConditional>(fg,keys);
+  list<varid_t> xvar; xvar.push_back(0);
+  GaussianBayesNet actual = *Inference::Marginal(GaussianFactorGraph(cbn), xvar);
 
   // expected is just scalar Gaussian on x
-  GaussianBayesNet expected = scalarGaussian("x", 4, sqrt(2));
+  GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2));
   CHECK(assert_equal(expected,actual));
 }