Added marginalGaussian to get a marginal on a single variable from a GaussianFactorGraph

cpp/inference-inl.h

@@ -8,6 +8,7 @@
 #include "FactorGraph-inl.h"
 #include "BayesNet-inl.h"
 #include "Key.h"
+#include "GaussianFactorGraph.h"
 
 using namespace std;
 
@@ -84,5 +85,37 @@ namespace gtsam {
 		}
 
 	/* ************************************************************************* */
+	pair<Vector,Matrix> marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key) {
+
+		// todo: this does not use colamd!
+
+		list<Symbol> ord;
+		BOOST_FOREACH(const Symbol& k, fg.keys()) {
+			if(k != key)
+				ord.push_back(k);
+		}
+		Ordering ordering(ord);
+
+		// Now make another factor graph where we eliminate all the other variables
+		GaussianFactorGraph marginal(fg);
+		marginal.eliminate(ordering);
+
+		GaussianFactor::shared_ptr factor;
+		for(size_t i=0; i<marginal.size(); i++)
+			if(marginal[i] != NULL) {
+				factor = marginal[i];
+				break;
+			}
+
+		if(factor->keys().size() != 1 || factor->keys().front() != key)
+			throw runtime_error("Didn't get the right marginal!");
+
+		VectorConfig mean_cfg(marginal.optimize(Ordering(key)));
+		Matrix A(factor->get_A(key));
+
+		return make_pair(mean_cfg[key], inverse(trans(A)*A));
+	}
+
+	/* ************************************************************************* */
 
 } // namespace gtsam

cpp/inference.h

@@ -14,6 +14,7 @@
 namespace gtsam {
 
 	class Ordering;
+	class GaussianFactorGraph;
 
 	// ELIMINATE: FACTOR GRAPH -> BAYES NET
 
@@ -50,4 +51,10 @@ namespace gtsam {
 	template<class Factor, class Conditional>
 	FactorGraph<Factor> marginalize(const BayesNet<Conditional>& bn, const Ordering& keys);
 
+	/**
+	 * Hacked-together function to compute a Gaussian marginal for the given variable.
+	 * todo: This is inefficient!
+	 */
+	std::pair<Vector,Matrix> marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key);
+
 } /// namespace gtsam