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

add a conversion function from GaussianFactorGraph to JacobianFactorGraph

release/4.3a0
Yong-Dian Jian 2012-09-06 01:21:15 +00:00
parent 4443752a18
commit 574a9711cc
3 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -635,4 +635,13 @@ break;
return e;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
result->push_back(convertToJacobianFactorPtr(gf));
}
return result;
}
} // namespace gtsam

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -354,4 +354,7 @@ namespace gtsam {
inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x); }
/** Convert all factors to JacobianFactor */
GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg);
} // namespace gtsam

4
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -17,8 +17,6 @@
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
using namespace std;
@ -28,7 +26,7 @@ namespace gtsam {
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
}
/* ************************************************************************* */