Added JacobianFactor constructor from a GFG that merges the factors

gtsam/linear/JacobianFactor.cpp

@@ -209,6 +209,22 @@ namespace gtsam {
     assertInvariants();
   }
 
+  /* ************************************************************************* */
+  JacobianFactor::JacobianFactor(const GaussianFactorGraph& gfg) : Ab_(matrix_) {
+    // Cast or convert to Jacobians
+    FactorGraph<JacobianFactor> jacobians;
+    BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
+      if(factor) {
+        if(JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
+          jacobians.push_back(jf);
+        else
+          jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
+      }
+    }
+
+    *this = *CombineJacobians(jacobians, VariableSlots(jacobians));
+  }
+
   /* ************************************************************************* */
   JacobianFactor& JacobianFactor::operator=(const JacobianFactor& rhs) {
     this->Base::operator=(rhs); // Copy keys

gtsam/linear/JacobianFactor.h

@@ -37,6 +37,7 @@ namespace gtsam {
   class HessianFactor;
   class VariableSlots;
   template<class C> class BayesNet;
+  class GaussianFactorGraph;
 
   /**
    * A Gaussian factor in the squared-error form.
@@ -133,6 +134,9 @@ namespace gtsam {
     /** Convert from a HessianFactor (does Cholesky) */
     JacobianFactor(const HessianFactor& factor);
 
+    /** Build a dense joint factor from all the factors in a factor graph. */
+    JacobianFactor(const GaussianFactorGraph& gfg);
+
     /** Virtual destructor */
     virtual ~JacobianFactor() {}