From 574a9711ccf83ba71660470fab5f7eed30d0751f Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Thu, 6 Sep 2012 01:21:15 +0000
Subject: [PATCH] add a conversion function from GaussianFactorGraph to
 JacobianFactorGraph

---
 gtsam/linear/GaussianFactorGraph.cpp    | 9 +++++++++
 gtsam/linear/GaussianFactorGraph.h      | 3 +++
 gtsam/linear/SubgraphPreconditioner.cpp | 4 +---
 3 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index ad31bd9e5..eda9732b2 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -635,4 +635,13 @@ break;
 		return e;
 	}
 
+	/* ************************************************************************* */
+	GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
+	  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+	  BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
+	    result->push_back(convertToJacobianFactorPtr(gf));
+	  }
+	  return result;
+	}
+
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 5e6e792be..b6486b3a6 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -354,4 +354,7 @@ namespace gtsam {
   inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
     return *gaussianErrors_(fg, x); }
 
+  /** Convert all factors to JacobianFactor */
+  GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg);
+
 } // namespace gtsam
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 72aa25554..04eed29d2 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -17,8 +17,6 @@
 
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-
 #include <boost/foreach.hpp>
 
 using namespace std;
@@ -28,7 +26,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
 			const sharedBayesNet& Rc1, const sharedValues& xbar) :
-		Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
+		Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
 	}
 
 	/* ************************************************************************* */