From 9fe0b66be5e2c86a69ef8b5441350a7f669e29c4 Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Tue, 19 Jun 2012 15:29:10 +0000
Subject: [PATCH] add a function that dynamically casts gaussian factor graph
 into jacobian factor graph

---
 gtsam/linear/JacobianFactorGraph.cpp | 15 +++++++++++++++
 gtsam/linear/JacobianFactorGraph.h   |  5 +++++
 2 files changed, 20 insertions(+)

diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp
index 632a55e02..0ca3b8667 100644
--- a/gtsam/linear/JacobianFactorGraph.cpp
+++ b/gtsam/linear/JacobianFactorGraph.cpp
@@ -102,6 +102,21 @@ void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, Vec
     ++i;
   }
 }
+
+/* ************************************************************************* */
+JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) {
+  JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
+  jfg->reserve(gfg.size());
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
+    JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
+    if(castedFactor) jfg->push_back(castedFactor);
+    else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested.");
+  }
+  return jfg;
+}
+
+
+
 } // namespace
 
 
diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h
index 598c69677..cb27b507a 100644
--- a/gtsam/linear/JacobianFactorGraph.h
+++ b/gtsam/linear/JacobianFactorGraph.h
@@ -19,8 +19,10 @@
 
 #include <gtsam/linear/Errors.h>
 #include <gtsam/inference/FactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
+#include <boost/shared_ptr.hpp>
 
 namespace gtsam {
 
@@ -65,4 +67,7 @@ namespace gtsam {
   void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
   void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
 
+  /** dynamic_cast the gaussian factors down to jacobian factors */
+  JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg);
+
 }