12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add a function that dynamically casts gaussian factor graph into jacobian factor graph

release/4.3a0
Yong-Dian Jian 2012-06-19 15:29:10 +00:00
parent 9cb903fe08
commit 9fe0b66be5
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/JacobianFactorGraph.cpp
View File

@ -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

5
gtsam/linear/JacobianFactorGraph.h
View File

@ -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);
}