From 1755136b1b8cc2b9ccd827eed12f2325c67310bc Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 26 Nov 2012 19:21:05 +0000
Subject: [PATCH] Added JacobianFactor constructor from a GFG that merges the
 factors

---
 gtsam/linear/JacobianFactor.cpp | 16 ++++++++++++++++
 gtsam/linear/JacobianFactor.h   |  4 ++++
 2 files changed, 20 insertions(+)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 2ae8b3eb1..7bc282557 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/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
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 61ba5f081..95cce138f 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/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() {}