Moved JacobianFactor type check/conversion functions into SubgraphSolver instead of GaussianFactorGraph and JacobianFactor

gtsam/linear/GaussianFactorGraph.cpp

@@ -523,6 +523,14 @@ break;
 
 
 
+	/* ************************************************************************* */
+	static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
+		JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+		if( !result ) {
+			result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+		}
+		return result;
+	}
 
 	/* ************************************************************************* */
 	Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
@@ -635,13 +643,4 @@ 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

gtsam/linear/GaussianFactorGraph.h

@@ -354,7 +354,4 @@ 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

gtsam/linear/JacobianFactor.cpp

@@ -489,13 +489,4 @@ namespace gtsam {
     return description_.c_str();
   }
 
-  /* ************************************************************************* */
-  JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
-    JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
-    if( !result ) {
-      result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
-    }
-    return result;
-  }
-
 }

gtsam/linear/JacobianFactor.h

@@ -324,8 +324,5 @@ namespace gtsam {
     }
   }; // JacobianFactor
 
-  /** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */
-  JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf);
-
 } // gtsam
 

gtsam/linear/SubgraphPreconditioner.cpp

@@ -23,6 +23,19 @@ using namespace std;
 
 namespace gtsam {
 
+	/* ************************************************************************* */
+	static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
+		GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+		BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
+			JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+			if( !jf ) {
+				jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+			}
+			result->push_back(jf);
+		}
+		return result;
+	}
+
 	/* ************************************************************************* */
 	SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
 			const sharedBayesNet& Rc1, const sharedValues& xbar) :

gtsam/linear/SubgraphPreconditioner.h

@@ -33,7 +33,6 @@ namespace gtsam {
 	class SubgraphPreconditioner {
 
 	public:
-
 	  typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
 		typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
 		typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;

gtsam/linear/SubgraphSolver.cpp

@@ -111,26 +111,24 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
   size_t t = 0;
   BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
 
-		JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf);
+    if ( gf->keys().size() > 2 ) {
-
-    if ( jf->keys().size() > 2 ) {
       throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
     }
 
     bool augment = false ;
 
     /* check whether this factor should be augmented to the "tree" graph */
-    if ( jf->keys().size() == 1 ) augment = true;
+    if ( gf->keys().size() == 1 ) augment = true;
     else {
-      const Index u = jf->keys()[0], v = jf->keys()[1],
+      const Index u = gf->keys()[0], v = gf->keys()[1],
                   u_root = D.findSet(u), v_root = D.findSet(v);
       if ( u_root != v_root ) {
         t++; augment = true ;
         D.makeUnionInPlace(u_root, v_root);
       }
     }
-    if ( augment ) At->push_back(jf);
+    if ( augment ) At->push_back(gf);
-    else Ac->push_back(jf);
+    else Ac->push_back(gf);
   }
 
   return boost::tie(At, Ac);