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

Moved JacobianFactor type check/conversion functions into SubgraphSolver instead of GaussianFactorGraph and JacobianFactor

release/4.3a0
Richard Roberts 2012-09-06 14:33:44 +00:00
parent ce3c774bfa
commit 6d1b86c2e0
7 changed files with 26 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -523,6 +523,14 @@ break;
/* ************************************************************************* */
static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result ) {
result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
return result;
}
/* ************************************************************************* */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
@ -635,13 +643,4 @@ 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,7 +354,4 @@ 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

9
gtsam/linear/JacobianFactor.cpp
View File

@ -489,13 +489,4 @@ namespace gtsam {
return description_.c_str();
}
/* ************************************************************************* */
JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result ) {
result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
return result;
}
}

3
gtsam/linear/JacobianFactor.h
View File

@ -324,8 +324,5 @@ namespace gtsam {
}
}; // JacobianFactor
/** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */
JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf);
} // gtsam

13
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -22,6 +22,19 @@
using namespace std;
namespace gtsam {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
result->push_back(jf);
}
return result;
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,

1
gtsam/linear/SubgraphPreconditioner.h
View File

@ -33,7 +33,6 @@ namespace gtsam {
class SubgraphPreconditioner {
public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;

12
gtsam/linear/SubgraphSolver.cpp
View File

@ -111,26 +111,24 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
size_t t = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf);
if ( jf->keys().size() > 2 ) {
if ( gf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
bool augment = false ;
/* check whether this factor should be augmented to the "tree" graph */
if ( jf->keys().size() == 1 ) augment = true;
if ( gf->keys().size() == 1 ) augment = true;
else {
const Index u = jf->keys()[0], v = jf->keys()[1],
const Index u = gf->keys()[0], v = gf->keys()[1],
u_root = D.findSet(u), v_root = D.findSet(v);
if ( u_root != v_root ) {
t++; augment = true ;
D.makeUnionInPlace(u_root, v_root);
}
}
if ( augment ) At->push_back(jf);
else Ac->push_back(jf);
if ( augment ) At->push_back(gf);
else Ac->push_back(gf);
}
return boost::tie(At, Ac);