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(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 #include +#include #include #include +#include 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); + }